function [est_pf,mu,Sigma,z_rbpf,w,BBt,par,z_rbpf_pred_v] = frq_rbpf3(x,t,fs,tao,gm,prs,amp,N,nfig,grnd) % PURPOSE : PF and RBPF for conditionally Gaussian frequency tracker if nargin< 8, N = 400; % Number of particles. end if nargin< 9, nfig = 0; % Number of particles. end flops(0); nf = 1; echo off; % ======================================================================= % INITIALISATION AND PARAMETERS % ======================================================================= y = x'; td = t(2) - t(1); T = length(y); % Number of time steps. % Here, we give you the choice to try three different types of % resampling algorithms: multinomial (select 3), residual (1) and % deterministic (2). Note that the code for these O(N) algorithms is generic. resamplingScheme = 2; n_x = nf; % Continuous state dimension. n_y = 1; % Dimension of observations. n_z = nf*2; indps = (1:nf)'; indfs = (nf+1:n_z)'; par.A = eye(n_x,n_x); par.C = zeros(n_y,n_x); % depending on z % par.B = 1e-2*eye(n_x,n_x)*td; % par.B = zeros(n_x,n_x); % BBt = par.B*par.B'; % the process noise var. BBt = tao(1,1); % par.D = 1e-2*eye(n_y,n_y); % DDt = par.D*par.D'; % the observation noise var. DDt = gm; par.T = kron([1 2*pi*td;0 1],eye(nf)); % Transition matrix. z_rbpf_pred_v = kron([0 0;0 tao(3,3)],eye(nf)); % ======================================================================= % RBPF ESTIMATION % ======================================================================= % INITIALISATION: % ============== z_rbpf = ones(n_z,T,N); % These are the particles for the estimate % of z. Note that there's no need to store % them for all t. We're only doing this to % show you all the nice plots at the end. z_rbpf_pred = ones(n_z,T,N); % One-step-ahead predicted values of z. mu = 10*rand(T,N); % Kalman mean of x. mu_pred = 5*rand(1,N); y_pred = 0.01*randn(T,N); % One-step-ahead predicted values of y. w = ones(T,N); % Importance weights. Sigma = 1e1*ones(N,T); Sigma_pred = 0.1*prs.v0(1,1)*ones(1,N); S = zeros(1,N); % Kalman predictive covariance. inz2 = linspace(0,fs/2,N); z_rbpf(:,1,:) = [2*pi*rand(1,N); inz2]; % par.C = sin( reshape(z_rbpf(1,1,:),1,N)); % nf should be 1 % S = par.C.^2 .*Sigma_pred+DDt; if 0 for i=1:N, z_rbpf(:,1,i) = [2*pi*rand(1); inz2(i)]; % par.C = sin(z_rbpf(1,1,i)+2*pi*td*z_rbpf(2,1,i)); par.C = sin(z_rbpf(indps,1,i))'; S(i) = par.C*Sigma_pred(i)*par.C'+DDt; end; end act_nodes=N*ones(T,1); for j=2:T, if mod(j,40)== 0, fprintf('RBPF : j = %i / %i \r',j,T); fprintf('\n'); end z_rbpf_prev_m = reshape(z_rbpf(2,j-1,:), 1, N); z_rbpf_pred(2,j,:) = normrnd(z_rbpf_prev_m, sqrt(z_rbpf_pred_v(indfs,indfs))); z_tp = reshape(z_rbpf(:,j-1,:), n_z, N); z_rbpf_pred(1,j,:) = par.T(1,:)*z_tp; z_tp = reshape(z_rbpf_pred(1,j,:), 1, N); rng_ind = find(abs(z_tp-mean(z_rbpf(1,j-1,:),3))>pi); % z_rbpf_pred(:,j,rng_ind) = z_rbpf_pred(:,j-1,rng_ind)+1e-7*rand(1); % z_rbpf_pred(:,j,rng_ind) = zeros(n_z,length(rng_ind)); % act_nodes(j) = act_nodes(j)-length(rng_ind); % Kalman prediction: mu_pred = mu(j-1,:); % Sigma_pred(:,:,i)=par.A*Sigma(:,:,i,j-1)*par.A'+BBt; Sigma_pred = Sigma(:,j-1)'+BBt; par.C = sin( reshape(z_rbpf_pred(1,j,:),1,N)); % nf should be 1 % par.C = sin(z_rbpf_pred(indps,j,i))'; % for i=1:N, S = par.C.^2 .*Sigma_pred+DDt; y_pred(j,:) = par.C .* mu_pred; % Evaluate importance weights. w(j,:) = -0.5*log(S) - ... 0.5*(y(j)-y_pred(j,:)).^2 ./ S ; % w(j,:) = exp(log_w(j,:))+ 1e-99*ones(size(w(j,:))); % w(j,:) = w(j,:).*w(j-1,:); expw = exp(w(j,:)); expw(rng_ind) = zeros(1,length(rng_ind)); w(j,:) = expw / sum(expw); % w(j,:) = exp( w(j,:)-logsumexp(w(j,:),2) ); % Normalise the weights. if 1/sum(w(j,:).^2)