EKF,UKF,PF三种算法的比较粒子滤波matlab仿真程序EKF,UKF,PF三种算法的比较matlab仿真程序

%EKF UKF PF的三个算法clear;%tic;x=0.1;%初始状态x_estimate=1;%状态的估计e_x_estimate=x_estimate;%EKF的初始估计u_x_estimate=x_estimate;%UKF的初始估计p_x_estimate=x_estimate;%PF的初始估计Q=10;%input('请输入过程噪声方差Q的值:');%过程状态协方差R=1;%input('请输入测量噪声方差R的值:');%测量噪声协方差P=5;%初始估计方差e_P=P;%UKF方差u_P=P;%UKF方差pf_P=P;%PF方差tf=50;%模拟长度x_array=[x];%真实值数组e_x_estimate_array=[e_x_estimate];%EKF最优估计值数组u_x_estimate_array=[u_x_estimate];%UKF最优估计值数组p_x_estimate_array=[p_x_estimate];%PF最优估计值数组u_k=1;%微调参数u_symmetry_number=4;%对称的点的个数u_total_number=2*u_symmetry_number+1;%总的采样点的个数linear=0.5;N=500;%粒子滤波的粒子数close all;%粒子滤波初始N个粒子for i=1:Np_xpart(i)=p_x_estimate+sqrt(pf_P)*randn;endfor k=1:tf%模拟系统x=linear*x+(25*x/(1+x^2))+8*cos(1.2*(k-1))+sqrt(Q)*randn;%状态值y=(x^2/20)+sqrt(R)*randn;%观测值%扩展卡尔曼滤波器%进行估计第一阶段的估计e_x_estimate_1=linear*e_x_estimate+25*e_x_estimate/(1+e_x_estimate^2)+8*cos(1.2*(k-1));e_y_estimate=(e_x_estimate_1)^2/20;%这是根据k=1时估计值为1得到的观测值；只是这个由我估计得到的第24行的y也是观测值不过是由加了噪声的真实值得到的%相关矩阵e_A=linear+25*(1-e_x_estimate^2)/((1+e_x_estimate^2)^2);%传递矩阵e_H=e_x_estimate_1/10;%观测矩阵%估计的误差e_p_estimate=e_A*e_P*e_A'+Q;%扩展卡尔曼增益e_K=e_p_estimate*e_H'/(e_H*e_p_estimate*e_H'+R);%进行估计值的更新第二阶段e_x_estimate_2=e_x_estimate_1+e_K*(y-e_y_estimate);%更新后的估计值的误差e_p_estimate_update=e_p_estimate-e_K*e_H*e_p_estimate;%进入下一次迭代的参数变化e_P=e_p_estimate_update;e_x_estimate=e_x_estimate_2;%粒子滤波器%粒子滤波器for i=1:Np_xpartminus(i)=0.5*p_xpart(i)+25*p_xpart(i)/(1+p_xpart(i)^2)+8*cos(1.2*(k-1))+ sqrt(Q)*randn;%这个式子比下面一行的效果好%xpartminus(i)=0.5*xpart(i)+25*xpart(i)/(1+xpart(i)^2)+8*cos(1.2*(k-1));p_ypart=p_xpartminus(i)^2/20;%预测值p_vhat=y-p_ypart;%观测和预测的差p_q(i)=(1/sqrt(R)/sqrt(2*pi))*exp(-p_vhat^2/2/R);%各个粒子的权值end%平均每一个估计的可能性p_qsum=sum(p_q);for i=1:Np_q(i)=p_q(i)/p_qsum;%各个粒子进行权值归一化end%重采样权重大的粒子多采点，权重小的粒子少采点,相当于每一次都进行重采样；for i=1:Np_u=rand;p_qtempsum=0;for j=1:Np_qtempsum=p_qtempsum+p_q(j);if p_qtempsum>=p_up_xpart(i)=p_xpartminus(j);%在这里xpart(i)实现循环赋值；终于找到了这里！！！break;endendendp_x_estimate=mean(p_xpart);%p_x_estimate=0;%for i=1:N%p_x_estimate=p_x_estimate+p_q(i)*p_xpart(i);%end%不敏卡尔曼滤波器%采样点的选取存在x(i)u_x_par=u_x_estimate;for i=2:(u_symmetry_number+1)u_x_par(i,:)=u_x_estimate+sqrt((u_symmetry_number+u_k)*u_P);endfor i=(u_symmetry_number+2):u_total_numberu_x_par(i,:)=u_x_estimate-sqrt((u_symmetry_number+u_k)*u_P);end%计算权值u_w_1=u_k/(u_symmetry_number+u_k);u_w_N1=1/(2*(u_symmetry_number+u_k));%把这些粒子通过传递方程得到下一个状态for i=1:u_total_numberu_x_par(i)=0.5*u_x_par(i)+25*u_x_par(i)/(1+u_x_par(i)^2)+8*cos(1.2*(k-1));end%传递后的均值和方差u_x_next=u_w_1*u_x_par(1);for i=2:u_total_numberu_x_next=u_x_next+u_w_N1*u_x_par(i);endu_p_next=Q+u_w_1*(u_x_par(1)-u_x_next)*(u_x_par(1)-u_x_next)';for i=2:u_total_numberu_p_next=u_p_next+u_w_N1*(u_x_par(i)-u_x_next)*(u_x_par(i)-u_x_next)';end%%对传递后的均值和方差进行采样产生粒子存在y(i)%u_y_2obser(1)=u_x_next;%for i=2:(u_symmetry_number+1)%u_y_2obser(i,:)=u_x_next+sqrt((u_symmetry_number+k)*u_p_next);%end%for i=(u_symmetry_number+2):u_total_number%u_y_2obser(i,:)=u_x_next-sqrt((u_symmetry_number+u_k)*u_p_next); %end%另外存在y_2obser(i)中；for i=1:u_total_numberu_y_2obser(i,:)=u_x_par(i);end%通过观测方程得到一系列的粒子for i=1:u_total_numberu_y_2obser(i)=u_y_2obser(i)^2/20;end%通过观测方程后的均值y_obseu_y_obse=u_w_1*u_y_2obser(1);for i=2:u_total_numberu_y_obse=u_y_obse+u_w_N1*u_y_2obser(i);end%Pzz测量方差矩阵u_pzz=R+u_w_1*(u_y_2obser(1)-u_y_obse)*(u_y_2obser(1)-u_y_obse)';for i=2:u_total_numberu_pzz=u_pzz+u_w_N1*(u_y_2obser(i)-u_y_obse)*(u_y_2obser(i)-u_y_obse)';end%Pxz状态向量与测量值的协方差矩阵u_pxz=u_w_1*(u_x_par(1)-u_x_next)*(u_y_2obser(1)-u_y_obse)';for i=2:u_total_numberu_pxz=u_pxz+u_w_N1*(u_x_par(i)-u_x_next)*(u_y_2obser(i)-u_y_obse)';end%卡尔曼增益u_K=u_pxz/u_pzz;%估计量的更新u_x_next_optimal=u_x_next+u_K*(y-u_y_obse);%第一步的估计值+修正值；u_x_estimate=u_x_next_optimal;%方差的更新u_p_next_update=u_p_next-u_K*u_pzz*u_K';u_P=u_p_next_update;%进行画图程序x_array=[x_array,x];e_x_estimate_array=[e_x_estimate_array,e_x_estimate];p_x_estimate_array=[p_x_estimate_array,p_x_estimate];u_x_estimate_array=[u_x_estimate_array,u_x_estimate];e_error(k,:)=abs(x_array(k)-e_x_estimate_array(k));p_error(k,:)=abs(x_array(k)-p_x_estimate_array(k));u_error(k,:)=abs(x_array(k)-u_x_estimate_array(k));endt=0:tf;figure;plot(t,x_array,'k.',t,e_x_estimate_array,'r-',t,p_x_estimate_array,'g--',t,u_x_estimate_array,'b:');set(gca,'FontSize',10);set(gcf,'color','White');xlabel('时间步长');%lable--->label我的神ylabel('状态');legend('真实值','EKF估计值','PF估计值','UKF估计值');figure;plot(t,x_array,'k.',t,p_x_estimate_array,'g--',t,p_x_estimate_array-1.96*sqrt(P),'r:',t, p_x_estimate_array+1.96*sqrt(P),'r:');set(gca,'FontSize',10);set(gcf,'color','White');xlabel('时间步长');%lable--->label我的神ylabel('状态');legend('真实值','PF估计值','95%置信区间');%root mean square平均值的平方根e_xrms=sqrt((norm(x_array-e_x_estimate_array)^2)/tf);disp(['EKF估计误差均方值=',num2str(e_xrms)]);p_xrms=sqrt((norm(x_array-p_x_estimate_array)^2)/tf);disp(['PF估计误差均方值=',num2str(p_xrms)]);u_xrms=sqrt((norm(x_array-u_x_estimate_array)^2)/tf);disp(['UKF估计误差均方值=',num2str(u_xrms)]);%plot(t,e_error,'r-',t,p_error,'g--',t,u_error,'b:');%legend('EKF估计值误差','PF估计值误差','UKF估计值误差');t=1:tf;figure;plot(t,e_error,'r-',t,p_error,'g--',t,u_error,'b:');set(gca,'FontSize',10);set(gcf,'color','White');xlabel('时间步长');%lable--->label我的神ylabel('状态');legend('EKF估计值误差','PF估计值误差','UKF估计值误差');%toc;。

EKFUKFPF算法的比较程序在估计理论中，EKF（Extended Kalman Filter），UKF（Unscented Kalman Filter）和PF（Particle Filter）是三种常用的非线性滤波算法。

它们在不同的环境和应用中具有不同的优点和缺点。

下面将对这三种算法进行比较。

首先，EKF是最常用的非线性滤波算法之一、它通过线性化状态转移方程和测量方程来近似非线性问题。

EKF在处理高斯噪声的情况下表现良好，但在处理非高斯噪声时会有较大的误差。

由于线性化过程的存在，EKF对于高度非线性和非高斯问题可能表现不佳。

此外，EKF对系统模型的准确性要求较高，较大的模型误差可能导致滤波结果的不准确性。

其次，UKF通过构造一组代表系统状态的Sigma点，通过非线性映射来近似非线性函数。

相较于EKF，UKF无需线性化系统模型，因此适用于更广泛的非线性系统。

UKF的优点是相对较好地处理了非线性系统和非高斯噪声，但在处理维数较高的问题时，计算开销较大。

最后，PF是一种基于粒子的滤波方法，通过使用一组代表系统状态的粒子来近似概率密度函数。

PF的优点是它可以处理非线性系统和非高斯噪声，并且在系统模型不准确或缺乏确定性时，具有较好的鲁棒性。

由于粒子的数量可以灵活调整，PF可以提供较高的估计精度。

然而，PF的计算开销较大，尤其在高维度的情况下。

综上所述，EKF、UKF和PF是三种常用的非线性滤波算法。

EKF适用于高斯噪声条件下的非线性问题，但对系统模型准确性要求高。

UKF适用于一般的非线性问题，但计算开销较大。

PF适用于非线性和非高斯噪声条件下的问题，并具有较好的鲁棒性，但在计算开销方面具有一定的挑战。

在实际应用中，我们应根据具体问题的性质和要求选择合适的算法。

比如，在低维情况下，EKF是一个可行的选择；在高维或非高斯噪声情况下，可以考虑使用UKF或PF算法。

龙源期刊网
几种非线性滤波算法的比较研究
作者：王庆欣史连艳
来源：《现代电子技术》2011年第06期
摘要：针对组合导航等非线性系统，扩展卡尔曼滤波算法(EKF)在初值不准确时存在滤波发散的现象，故提出U-卡尔曼滤波(UKF)；粒子滤波算法(PF)适合于强非线性、非高斯噪声系统，但同时存在退化现象，故提出2种改进算法。

前人的工作多集中在单一算法的研究，而在此是将上述各种算法应用到同一典型非线性系统，通过应用Matlab进行仿真实验得出具体滤波效果数据，综合对比分析了各算法的优缺点。

得出一些有用的结论，为组合导航系统中非线性滤波算法的选择提供了参考。

关键词：卡尔曼滤波；粒子滤波；非线性滤波算法；导航系统。

一、介绍粒子滤波算法粒子滤波算法是一种基于蒙特卡洛方法的非线性、非高斯滤波算法，它通过一组随机产生的粒子来近似表示系统的后验概率分布，从而实现对非线性、非高斯系统的状态估计。

在实际应用中，粒子滤波算法被广泛应用于目标跟踪、导航、机器人定位等领域。

本文将以matlab 实例的形式介绍粒子滤波算法的基本原理和应用。

二、粒子滤波算法的原理及步骤粒子滤波算法的主要原理是基于贝叶斯滤波理论，通过一组随机产生的粒子来近似表示系统的后验概率分布。

其具体步骤如下：1. 初始化：随机生成一组粒子，对于状态变量的初始值和方差的估计，通过随机抽样得到一组粒子。

2. 预测：根据系统模型，对每个粒子进行状态预测，得到预测状态。

3. 更新：根据测量信息，对每个预测状态进行权重更新，得到更新后的状态。

4. 重采样：根据更新后的权重，对粒子进行重采样，以满足后验概率分布的表示。

5. 输出：根据重采样后的粒子，得到对系统状态的估计。

三、粒子滤波算法的matlab实例下面以一个简单的目标跟踪问题为例，介绍粒子滤波算法在matlab中的实现。

假设存在一个目标在二维空间中运动，我们需要通过一系列测量得到目标的状态。

我们初始化一组粒子来近似表示目标的状态分布。

我们根据目标的运动模型，预测每个粒子的状态。

根据测量信息，对每个预测状态进行权重更新。

根据更新后的权重，对粒子进行重采样，并输出对目标状态的估计。

在matlab中，我们可以通过编写一段简单的代码来实现粒子滤波算法。

我们需要定义目标的运动模型和测量模型，然后初始化一组粒子。

我们通过循环来进行预测、更新、重采样的步骤，最终得到目标状态的估计。

四、总结粒子滤波算法是一种非线性、非高斯滤波算法，通过一组随机产生的粒子来近似表示系统的后验概率分布。

在实际应用中，粒子滤波算法被广泛应用于目标跟踪、导航、机器人定位等领域。

本文以matlab实例的形式介绍了粒子滤波算法的基本原理和应用，并通过一个简单的目标跟踪问题，展示了粒子滤波算法在matlab中的实现过程。

利用EKF、CMKF和UKF算法处理模型非线性的性能比较1 概述地空导弹武器系统的制导雷达对空中目标进行探测跟踪，可独立测得极坐标下的目标斜距、高低角和方位角；目标运动的状态可由位置、速度和加速度三变量在直角坐标系中线性地表示。

由于极坐标与直角坐标系之间存在非线性的变换关系，使得在对运动目标的跟踪中，目标的运动状态方程和量测方程存在非线性方程。

通常为了更直观和方便的描述目标运动过程，将目标运动状态方程选在直角坐标系中建立，则量测方程为非线性形式，对非线性量测方程的处理方式影响了滤波器的跟踪特性。

本文主要针对目前EKF、CMKF和UKF三种对非线性问题的处理方法从滤波性能、滤波系统跟踪特性以及计算量等方面进行对比分析，利于滤波算法的合理选择，改善地空导弹武器系统对目标的探测跟踪性能。

2 直角坐标系下的目标运动模型由于地空导弹武器系统所拦截的空中目标存在转弯、逃避等多种机动飞行情况，为更为真实的反映目标机动范围和强度的变化，采用非零均值和修正瑞利分布表征机动加速度特性，因此，建立在直角坐标系下的基于机动目标“当前”统计模型的目标的状态方程。

3 量测方程非线性的处理方法（1）EKF滤波算法（2）CMKF滤波算法（3）UKF滤波算法UKF滤波算法对卡尔曼滤波中的一步预测方程，使用无迹（UT）变换来处理均值和协方差的非线性传递。

UT变换的基本思路为：将被估计状态视为随机变量，利用采样策略，在原先状态分布中选取一些点，使其均值和协方差与状态分布的均值和协方差相等，将这些点代入非线性函数中，得到非线性函数值点集，然后在测量的基础上调节样本点的位置，使得样本均值和样本方差分别以二次精度逼近实际分布的均值和方差，得到非线性系统方程和量测方程的均值和方差后，利用卡尔曼线性滤波框架中的测量更新，可获得状态变量的估计值。

4 仿真比较针对目标飞行速度600m/s、飞行高度6km、初始斜距21km，目标水平蛇形机动飞行；制导雷达测量的目标斜距的量测噪声服从（0，25m）的高斯分布，高低和方位角度噪声服从（0，0.2mrad）高斯分布，目标运动模型中的系统方差Q和量测方差R值相同，对比采用EKF、CMKF以及UKF对量测方差非线性处理下的性能，如下图1至图5所示。

EKF、UKF、PF组合导航算法仿真对比分析摘要随着人类对海洋探索的逐步深入，自主式水下机器人已被广泛地应用于海底搜救、石油开采、军事研究等领域。

良好的导航性能可以为航行过程提供准确的定位、速度和姿态信息，有利于AUV精准作业和安全回收。

本文介绍了三种不同的导航算法的基本原理，并对算法性能进行了仿真实验分析。

结果表明，在系统模型和时间步长相同的情况下，粒子滤波算法性能优于无迹卡尔曼滤波算法，无迹卡尔曼滤波算法性能优于扩展卡尔曼滤波算法。

关键词自主式水下机器人导航粒子滤波无迹卡尔曼滤波扩展卡尔曼滤波海洋蕴藏着丰富的矿产资源、生物资源和其他能源，但海洋能见度低、环境复杂、未知度高，使人类探索海洋充满了挑战。

自主式水下机器人（Autonomous Underwater Vehicle，AUV)可以代替人类进行海底勘探、取样等任务[1]，是人类探索和开发海洋的重要工具，已被广泛地应用于海底搜救、石油开采、军事研究等领域。

为了使其具有较好的导航性能，准确到达目的地，通常采用组合导航算法为其导航定位。

常用的几种组合导航算法有扩展卡尔曼滤波算法（Extended Kalman Filter，EKF）、无迹卡尔曼滤波算法（Unscented Kalman Filter，UKF）和粒子滤波算法（Particle Filter，PF）。

1扩展卡尔曼滤波算法EKF滤波算法通过泰勒公式对非线性系统的测量方程和状态方程进行一阶线性化截断，主要包括预测阶段和更新阶段。

预测阶段是利用上一时刻的状态变量和协方差矩阵来预测当前时刻的状态变量和协方差矩阵；更新阶段是通过计算卡尔曼增益，并结合预测阶段更新的状态变量和当前时刻的测量值，进而更新状态变量和协方差矩阵[2]。

虽然EKF滤波算法在非线性状态估计系统中广泛应用，但也凸显出两个问题：一是由于泰勒展开式抛弃了高阶项导致截断误差产生，所以当系统处于强非线性、非高斯环境时，EKF算法可能会使滤波发散；二是由于EKF算法在线性化处理时需要用雅克比(Jacobian)矩阵，其繁琐的计算过程导致该方法实现相对困难。