粒子滤波matlab代码.m

粒子滤波代码

[xk,wk]=m_SIR(Ns, xks1, wks1, zk)
%Ns:        粒子数目
%xks1:      存储k-1时刻各粒子状态列向量的矩阵 xks1(xdim,Ns), xdim表示粒子状态列向量维数
%wks1:      存储k-1时刻各粒子权重列向量 wks(Ns,1)
%zk:        存储k时刻的测量列向量
%xpk:       存储k时刻未重采样前各粒子状态列向量的矩阵 xpk(xdim,Ns)
[xdim,temp]=size(xks1);
xpk=zeros(xdim,Ns);
%xk:        存储k时刻未重采样前各粒子状态列向量的矩阵 xk(xdim,Ns)
xk=zeros(xdim,Ns);
%wpk:       存储k时刻未重采样前各粒子权重列向量wpk(Ns,1)
wpk=zeros(Ns,1);
%wk:        存储k时刻未重采样前各粒子权重列向量wk(Ns,1)
wk=zeros(Ns,1);
for i=1:1:Ns
   %   xp(i,k)~p(x(k)|x(i,k-1)
   xpk(:,i)=m_McState(xks1(:,i));   %%%%注此自己定义
   %   wp(i,k)=p(z(k)|xp(i,k)       %%%%注此自己定义
   wpk(i)=m_Pdf(zk, xpk(:,i));
end
%calculate total weight
temp=sum(wpk(i));
%Normalize
wpk=wpk/temp;
%%%%%%%%%%%%%%%%%%%%%%%%%Resampling algorithm%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%[{x(j*,k),w(j*,k),parent(j)},j=1,..,Ns]=Resample[{x(i,k),w(i,k)},i=1,...,Ns]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
parent=zeros(Ns,1);
c=zeros(Ns,1);
%initialize the cdf
c(1)=wpk(1);                    %%%%注原文是c(1)=wpk(0)  
for i=2:1:Ns
    c(i)=c(i)+wpk(i);
end
u=zeros(Ns,1);
%Draw a starting point:
u(1)=rand(1,1/Ns);
for j=1:1:Ns
    u(j)=u(1)+(j-1)/Ns;
    j=1;
    while u(j)>c(i)
        j=j-1;
    end
    xk(:,j)=xpk(:,i);
    wk(j)=1/Ns;
    parent(j)=i;
end