改进粒子滤波算计及其matlab实现

Github个人博客：https://joeyos.github.io

基本粒子滤波存在的问题

基本粒子滤波算法中普遍存在的问题是退化现象，这是因为粒子权值的方差会随着时间的迭代而不断增加。退化现象是不可避免的，经过若干次迭代，除了少数粒子外，其他粒子的权值可以忽略不计。退化的意味着如果继续跌代下去，那么大量的计算资源就会消耗在处理那些微不足道的粒子上。可以采取如下措施避免：

（1）增加粒子数
增加粒子数也叫增加采样点，粒子数目多，自然能全面反映粒子多样性，能延缓退化，但运算量增大。

（2）重采样的本质是增加粒子的多样性。SIR粒子滤波在这点上做得比SIS粒子滤波成功。引入重采样机制，基本上避免了粒子丧失多样性的可能。重采样技术和选择合理的建议密度是同时采用的。

（3）选择合理的建议密度
基本粒子滤波的前提假设：重要性重采样能够从一个合理的后验建议密度分布中采样得到一组样本点集合，而且这组样本点集合能够很好地覆盖真实状态。如果这些假设条件不能满足，粒子滤波算法的效果就要下降；因此，如果能找到一个最优的建议密度分布函数，引导重采样做正确的采样分布，那么就能保证样本集合的有效性，也就保证了滤波的最终质量。

建议密度函数

介绍两种从建议密度函数的角度来改进粒子滤波算法的方法，分别用扩展卡尔曼EKF和无迹卡尔曼UKF来做建议密度函数，从而改进算法性能。

EPF算法

扩展卡尔曼是一种局部线性化的方法，它通过一阶泰勒展开式实现，它是一种递归的最小均方误差MMSE估计方法，要求系统是近似高斯后验分布模型。在粒子滤波算法框架下，EKF算法主要用于为每个粒子产生符合建议密度分布，称为扩展卡尔曼粒子滤波（EPF）。初始化-重要性采样-重采样-输出。

UPF算法

无迹卡尔曼滤波也是一种递归的最小均方误差估计，利用UKF来改进粒子滤波算法称为无迹卡尔曼粒子滤波（UPF）。初始化-重要性采样-重采样-输出。

UPF算法的误差较低，但计算时间较长。

算法实现

1.main.m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 主函数
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 功能说明：ekf,ukf,pf,epf,upf算法的综合比较程序
function main
 
rand('seed',3);
randn('seed',6);
%error('下面的参数T请参考书中的值设置，然后删除本行代码') 
T = 50;
R =  1e-5;              
                       
g1 = 3;                
g2 = 2;               
 
X = zeros(1,T);
Z = zeros(1,T);
processNoise = zeros(T,1);
measureNoise = zeros(T,1);
X(1) = 1;                         
P0 = 3/4;
 
Qekf=10*3/4;                
Rekf=1e-1;                    
Xekf=zeros(1,T);               
Pekf = P0*ones(1,T);         
Tekf=zeros(1,T);               

 
Qukf=2*3/4;                  
Rukf=1e-1;                    
Xukf=zeros(1,T);               
Pukf = P0*ones(1,T);          
Tukf=zeros(1,T);                

 
N=200;                      
Xpf=zeros(1,T);             
Xpfset=ones(T,N);         
Tpf=zeros(1,T);            

 
Xepf=zeros(1,T);           
Xepfset=ones(T,N);         
Pepf = P0*ones(T,N);          
Tepf=zeros(1,T);               

 
Xupf=zeros(1,T);            
Xupfset=ones(T,N);              
Pupf &