理论基础1：
(a) Prediction
- Use the transition equation to propagate the particles:
(b) Update
- Use the measurement equation to obtain measurements of the propagated particles and their standard deviations:
(in the case of our program, ym is obtained via simulation of the system, in real-time applications this value could be just the average of measurements).
- Evaluate the measurement PDF at the propagated particles. Then, normalize to obtain the weights. For instance:
(the weights have been denoted as pq(n) in the program)
- Resampling. Obtain new p ‾ x ( n + 1 ) \overline{p}x(n + 1) px(n+1) from a p ‾ x ( n + 1 ) a\overline{p}x (n + 1) apx(n+1)using the normalized weights.
Note: Several resampling methods:
1、Multinomial Resampling
%Resampling (multinomial)
acq=cumsum(pq);
mm=1;
nr=sort(rand(1,Np)); %ordered random numbers (0, 1]
for ip=1:Np
while(acq(mm)<nr(ip))
mm=mm+1;
end
aux=apx(:,mm);
Mpx(:,ip)=aux+(sig.*rn(:,ip)); %roughening
end
2、Systematic Resampling
%Resampling (systematic)
acq=cumsum(pq);
cmb=linspace(0,1-(1/Np),Np)+(rand(1)/Np); %the "comb"
cmb(Np+1)=1;
ip=1; mm=1;
while(ip<=Np)
if (cmb(ip)<acq(mm))
aux=apx(:,mm);
Spx(:,ip)=aux+(sig.*rn(:,ip)); %roughening
ip=ip+1;
else
mm=mm+1;
end
end
3、 Stratified Resampling
%Resampling (stratified)
acq=cumsum(pq);
stf=zeros(1,Np);
nr=rand(1,Np)/Np;
j=1:Np;
stf(j)=nr(j)+((j-1)/Np); %(vectorized code)
stf(Np+1)=1;
ip=1; mm=1;
while(ip<=Np)
if (stf(ip)<acq(mm))
aux=apx(:,mm);
Fpx(:,ip)=aux+(sig.*rn(:,ip)); %roughening
ip=ip+1;
else
mm=mm+1;
end
end
4、Residual Resampling
%Resampling (residual)
acq=cumsum(pq);
mm=1;
%preparation
na=floor(Np*pq); %repetition counts
NR=sum(na); %total count
Npr=Np-NR; %number of non-repeated particles
rpq=((Np*pq)-na)/Npr; %modified weights
acq=cumsum(rpq); %for the monomial part
%deterministic part
mm=1;
for j=1:Np
for nn=1:na(j)
Rpx(:,mm)=apx(:,j);
mm=mm+1;
end
end
%multinomial part:
nr=sort(rand(1,Npr)); %ordered random numbers (0, 1]
for j=1:Npr
while(acq(mm)<nr(j))
mm=mm+1;
end
aux=apx(:,mm);
Rpx(:,NR+j)=aux+(sig.*rn(:,j)); %roughening
end
一些比较研究表明，分层和系统重采样在滤波器估计和计算复杂度方面优于多项重采样。在残差重采样的情况下，大约一半的副本是确定的，另一半是随机的:这意味着更少的计算，但可以通过重新计算权重和算法的其他方面来补偿。部分实验证明，残差重采样的结果方差更小。
- Roughening
为了防止样本贫化，对后验粒子进行粗化处理[21,96]。添加一个零均值噪声，其方差正比于:
where n is the dimension of the space, k is a tuning parameter, and the j-th element of the vector M is:
where xm k (j) is the j-th component of the posterior particle xm k (which is a vector).
- Evaluate the mean of the set p ‾ x ( n + 1 ) \overline{p}x(n + 1) px(n+1) This is the estimated state
代码实现1：
%Particle filter example
%Radar monitoring of falling body
disp('please wait a bit');
%------------------------------------------
%Prepare for the simulation of the falling body
T=0.4; %sampling period
g=-9.81;
rho0=1.225; %air density, sea level
k=6705.6; %density vs. altitude constant
L=100; %horizontal distance radar<->object
L2=L^2;
Nf=100; %maximum number of steps
rx=zeros(3,Nf); %space for state record
tim=0:T:(Nf-1)*T; %time
%process noise
Sw=[10^5 0 0; 0 10^3 0; 0 0 10^2]; %cov
w11=sqrt(Sw(1,1)); w22=sqrt(Sw(2,2)); w33=sqrt(Sw(3,3));
w=[w11; w22; w33];
%observation noise
Sv=10^6; %cov.
v11=sqrt(Sv);
%------------------------------------------
%Prepare for filtering
%space for recording er(n), xe(n)
rer=zeros(3,Nf); rxe=zeros(3,Nf);
%------------------------------------------
%Behaviour of the system and the filter after initial state
x=[10^5; -5000; 400]; %initial state
xe=x; %initial estimation
%prepare particles
Np=1000; %number of particles
%reserve space
px=zeros(3,Np); %particles
apx=zeros(3,Np); %a priori particles
ny=zeros(1,Np); %particle measurements
vy=zeros(1,Np); %meas. dif.
pq=zeros(1,Np); %particle likelihoods
%particle generation
wnp=randn(3,Np); %noise (initial particles)
for ip=1:Np,
px(:,ip)=x+(w.*wnp(:,ip)); %initial particles
end
%system noises
wx=randn(3,Nf); %process
wy=randn(1,Nf); %output
nn=1;
while nn<Nf+1
%estimation recording
rxe(:,nn)=xe; %state
rer(:,nn)=x-xe; %error
%Simulation of the system
%system
rx(:,nn)=x; %state recording
rho=rho0*exp(-x(1)/k); %air density
d=(rho*(x(2)^2))/(2*x(3)); %drag
rd(nn)=d; %drag recording
%next system state
x(1)=x(1)+(x(2)*T);
x(2)=x(2)+((g+d)*T);
x(3)=x(3);
x=x+(w.*wx(:,nn)); %additive noise
%system output
y=sqrt(L2+(x(1)^2))+(v11*wy(nn)); %additive noise
ym=y; %measurement
%Particle propagation
wp=randn(3,Np); %noise (process)
vm=randn(1,Np); %noise (measurement)
for ip=1:Np
rho=rho0*exp(-px(1,ip)/k); %air density
d=(rho*(px(2,ip)^2))/(2*px(3,ip)); %drag
%next state
apx(1,ip)=px(1,ip)+(px(2,ip)*T);
apx(2,ip)=px(2,ip)+((g+d)*T);
apx(3,ip)=px(3,ip);
apx(:,ip)=apx(:,ip)+(w.*wp(:,ip)); %additive noise
%measurement (for next state)
ny(ip)=sqrt(L2+(apx(1,ip)^2))+(v11*vm(ip)); %additive noise
vy(ip)=ym-ny(ip);
end
%Likelihood
%(vectorized part)
%scaling
vs=max(abs(vy))/4;
ip=1:Np;
pq(ip)=exp(-((vy(ip)/vs).^2));
spq=sum(pq);
%normalization
pq(ip)=pq(ip)/spq;
%Prepare for roughening
A=(max(apx')-min(apx'))';
sig=0.2*A*Np^(-1/3);
rn=randn(3,Np); %random numbers
%================================
%Resampling (systematic)
acq=cumsum(pq);
cmb=linspace(0,1-(1/Np),Np)+(rand(1)/Np); %the "comb"
cmb(Np+1)=1;
ip=1; mm=1;
while(ip<=Np)
if (cmb(ip)<acq(mm))
aux=apx(:,mm);
px(:,ip)=aux+(sig.*rn(:,ip)); %roughening
ip=ip+1;
else
mm=mm+1;
end
end
%=================================
%Results
%estimated state (the particle mean)
xe=sum(px,2)/Np;
nn=nn+1;
end
%------------------------------------------
%display
figure(1)
subplot(1,2,1)
plot(tim,rx(1,1:Nf),'kx'); hold on;
plot(tim,rxe(1,1:Nf),'r');
title('altitude'); xlabel('seconds')
axis([0 Nf*T 0 12*10^4]);
subplot(1,2,2)
plot(tim,rx(2,1:Nf),'kx'); hold on;
plot(tim,rxe(2,1:Nf),'r');
title('velocity'); xlabel('seconds');
axis([0 Nf*T -6000 1000]);
结果：
参考：
Kalman Filter, Particle Filter and Other Bayesian Filters ↩︎ ↩︎
