pf

自己编的有关粒子子滤波的matlab程序

% an example of Generic Particle Filter
% Acorrding to the article "A Tutorial on Particle Filters for Online
% Nonlinear/Non-Gaussian Bayesian Tracking"by M.Sanjeev Arulampalam, Simon Maskell,
% Nell Gordon,and Tim Clapp
% IEEE TRANSACTIONS ON SIGNAL PROCESSING,VOL. 50, NO. 2, FEBRUARY 2002

% (1)生成初始粒子群，讲初始粒子设为均匀分布
% 设系统的状态方程为x(k)=x(k-1)+w(k-1)，其中，w(k)满足均值为0，方差为1的高斯分布。
% 设系统的测量方程为z(k)= x(k)+v(k)，其中v(k)满足均值为0，方差为4的高斯分布。
% 设置粒子数目,
particlenumber=200;
% 并且设定粒子初始权重，权重之和为1
everyeight=zeros(1,particlenumber);
for i=1:1:particlenumber
    everyeight(1,i)=1/particlenumber
end
% 初始化粒子的值：
particlevalue = linspace(50,500,particlenumber);
%初始化系统噪声
wk = 10 * random('norm',0,1,1,particlenumber);
hk = zeros(1,particlenumber);                  %初始化

%subplot(2,2,1);
%plot(1:200,everyeignt);
%axis([0 200 0 0.06]);

nt=1000;
%保存滤波误差方差
filtererror=zeros(1,1000);
%真值
x=zeros(1,1000);
%测量值
yc=zeros(1,1000);
%输出的状态估计值
Ex=zeros(1,1000);

%定义滤波
t = [1:1000]';
u = 5*sin(t/5);
v = sqrt(2)*randn(1,length(t));
x = [u + v']';


%设计重采样
cdf = zeros(1,particlenumber);
u1 = random('Uniform',0,1/particlenumber);
xx = zeros(1,particlenumber);      %初始化重采样后的粒子
weveryeight = zeros(1,particlenumber);    %初始化重采样后的粒子权值


t0 = clock;     %计算时间
    
for i = 1:nt
             
    % （2）时间更新 x(i,k) = f(x(i,k-1),w(i,k-1))------------------------------------------                                                                                          ---------
            wk = 5 * random('norm',0,1,1,particlenumber);
            particlevalue = particlevalue + wk; 
            %  经研究得到：如果样本{ x(i,k-1) }服从p(x(k-1)|y(k-1)),而 w(i,k-1)服从p(w(k-1)),那么对N个样本
             %  重复计算 x( k ) = x( k-1 ) + w( k -1 ) 得到的{ x(i,k) } 将以p(x(k)|y(k-1)为概率密度的分布
        
    %  （3）测量更新：在得到新的测量y(k)后，计算各个粒子的归一化-----------------------------------
                        %权值 w(i) = p(y(k)|x(i,k))/sum(p(y(k)|x(i,k)))
            yk = x(1,i) + random('norm',0,4);       %模拟产生测量值yk,由真值（加了噪声的真值）加测量随机噪声产生。
            
            yc(1,i) = yk;  %保存测量序列
            
            for ti = 1:particlenumber
               everyeight(1,ti) = exp(-(yk - particlevalue(1,ti))^2 * (1/8) ) / (sqrt((( 2 * pi )^2) * 4));             
            end
        
            sumeveryeight = sum(everyeight);
            
            everyeight = everyeight/sumeveryeight;  %得到的标准化后的粒子权值
        
            Ex(1,i) = dot(everyeight,particlevalue);  %保存每次的状态估计值
            
    %   （4）重新采样--------------------------------------------------------------------------------
            %创建CDF
             cdf(1,1) = everyeight(1,1);
             for tn = 2:particlenumber
                 cdf(1,tn) = cdf(1,tn-1) + everyeight(1,tn);
             end 
                      
             %创建新的粒子
             u1 = random('Uniform',0,1/particlenumber);
             tb = 1;
             for tk = 1:particlenumber
                 uj = u1 + ( tk -1)/particlenumber;
                 
                 while uj > cdf(1,tb)
                    tb = tb + 1;
                 end 
                 
              xx(1,tk) = particlevalue(1,tb);
              weveryeight(1,tk) = 1/paticlenumber
             end
             
            particlevalue = xx;
            everyeight = weveryeight
          %------------------------------------------------------------------------------------------
  end
  
  etime(clock,t0)       %%计算时间
  
  subplot(2,1,1);
   plot(1:nt,Ex,'r',1:nt,u,'b',1:nt,yc,'+g');
   legend('估计值','真值','测量值');
   title('Particle Filtering');
   grid on;
   
   
   for vt = 1:nt
          varp(1,vt) = (Ex(1,vt) - x(1,vt))^2;
   end    
   subplot(2,1,2);
   plot(1:nt,varp,'r');
   title('Variance of Particle Filtering');

   grid on;