MATLAB Implementing Particle Filter with Matlab

[manchung214]

I'm now implementing different kinds of Particle Filters using Matlab. One is the Regularized Particle Filter(RPF) and my code for 1 time-step filtering is as follows:

function [xcap,wcap]=RPF(x,z,k,N_thre,Q)
N=size(x,1); %no. of particles
m=size(x,2); %dimension of state space
xcap=zeros(N,1);
wcap=zeros(N,1);
%Particle Propagation
for i=1:N
xcap(i)=draw_px(x(i),k,Q);
wcap(i)=pz(z,xcap(i));
end
%Normalizing Weights
t=sum(wcap);
wcap=wcap./t;
N_eff=1/(sum(wcap.^2)); %Calculating Effective Sample Size
if (N_eff<N_thre)
S=emp_cov(xcap,wcap); %empirical covariance matrix
L=chol(S,'lower'); %the square root matrix of S
[xcap,wcap,~]=resample(xcap,wcap);
epsilon=zeros(N,m);
A=(4/(m+2))^(1/(m+4));
h=A*(N^(-1/(m+4)));
for i=1:N
epsilon(i)=(h*L)*randn(1,m);
xcap(i)=xcap(i)+h.*(L*(epsilon(i)'))';
end
end

In my experiment, I use the typical nonlinear system:
x(k)=0.5*x(k-1) + (25*x(k-1)/(1+x(k-1)^2)) + 8*cos(1.2(k-1)) + w(k)
and the measurement equation is:
z(k)=(x(k)^2)/20 + v(k)
where w and v are both zero-mean, white and Gaussian with variance Q and 1 respectively.

The function draw_px is used for sampling from the distribution p(x(k)|x(k-1)) and the function pz is used for calculating p(z(k)|x(k)).
My problem is that when calculating the weights, wcap, of the particles, pz often returns a number so close to 0 that computer recognizes it as 0. And sometimes, all the wcap(i)'s become 0 and in the normalizing step, wcap=wcap./t returns NaN because the sum t is 0.

Actually, the same problem occurs when implementing SIR particle filter. But in the SIR particle filter, resampling step is always performed and the weights will all be reset to 1/N after resampling step and hence the problem is hidden.

Is there anything wrong with my implementation of the RPF? If no, how could I fix the above problem?

Thank you!

 

[Khaled31000]

hello , i d ont understand this step epsilon(i)=(h*L)*randn(1,m);
how to draw from epsilon from the gaussian kernel