扩展Kalman滤波(EKF)和无迹卡尔曼滤波(ukf)

EKF与UKF一、背景普通卡尔曼滤波是在线性高斯情况下利用最小均方误差准则获得目标的动态估计，适应于过程和测量都属于线性系统，且误差符合高斯分布的系统。但是实际上很多系统都存在一定的非线性，表现在过程方程（状态方程）是非线性的，或者观测与状态之间的关系（测量方程）是非线性的。这种情况下就不能使用一般的卡尔曼滤波了。解决的方法是将非线性关系进行线性近似，将其转化成线性问题。对于非线性问题线性化常用的两大途径：（1）将非线性环节线性化，对高阶项采用忽略或逼近措施；（EKF）（2）用采样方法近似非线性分布.(UKF)二、扩展Kalman滤波(EKF)算法EKF算法是一种近似方法，它将非线性模型在状态估计值附近作泰勒级数展开，并在一阶截断，用得到的一阶近似项作为原状态方程和测量方程近似表达形式，从而实现线性化同时假定线性化后的状态依然服从高斯分布，然后对线性化后的系统采用标准卡尔曼滤波获得状态估计。采用局部线性化技术，能得到问题局部最优解，但它能否收敛于全局最优解，取决于函数的非线性强度以及展开点的选择。二、扩展Kalman滤波(EKF)算法假定定位跟踪问题的非线性状态方程和测量方程如下:在最近一次状态估计的时刻，对以上两式进行线性化处理，首先构造如下2个矩阵:)2(....................)()1.....(..........)(1kkkkkkVXhYWXfX)4.(..........)()()3.......()()1()1()(kkXXkkkXXkXXhKHXXfkkF二、扩展Kalman滤波(EKF)算法将线性化后的状态转移矩阵和观测矩阵代入到标准卡尔曼滤波框架中，即得到扩展卡尔曼滤波。因为EKF忽略了非线性函数泰勒展开的高阶项，仅仅用了一阶项，是非线性函数在局部线性化的结果，这就给估计带来了很大误差，所以只有当系统的状态方程和观测方程都接近线性且连续时，EKF的滤波结果才有可能接近真实值。EKF滤波结果的好坏还与状态噪声和观测噪声的统计特性有关，在EKF的递推滤波过程中，状态噪声和观测噪声的协方差矩阵保持不变，如果这两个噪声协方差矩阵估计的不够准确，那就容易产生误差累计，导致滤波器发散。EKF的另外一个缺点是初始状态不太好确定，如果假设的初始状态和初始协方差误差较大，也容易导致滤波器发散。二、扩展Kalman滤波(EKF)算法Matlab程序：functiontest_ekfkx=.01;ky=.05;%阻尼系数g=9.8;%重力t=10;%仿真时间Ts=0.1;%采样周期len=fix(t/Ts);%仿真步数%真实轨迹模拟dax=1.5;day=1.5;%系统噪声X=zeros(len,4);X(1,:)=[0,50,500,0];%状态模拟的初值fork=2:lenx=X(k-1,1);vx=X(k-1,2);y=X(k-1,3);vy=X(k-1,4);x=x+vx*Ts;vx=vx+(-kx*vx^2+dax*randn(1,1))*Ts;y=y+vy*Ts;二、扩展Kalman滤波(EKF)算法vy=vy+(ky*vy^2-g+day*randn(1))*Ts;X(k,:)=[x,vx,y,vy];endfigure(1),holdoff,plot(X(:,1),X(:,3),'-b'),gridon%figure(2),plot(X(:,2:2:4))%构造量测量mrad=0.001;dr=10;dafa=10*mrad;%量测噪声fork=1:lenr=sqrt(X(k,1)^2+X(k,3)^2)+dr*randn(1,1);a=atan(X(k,1)/X(k,3))+dafa*randn(1,1);Z(k,:)=[r,a];end二、扩展Kalman滤波(EKF)算法figure(1),holdon,plot(Z(:,1).*sin(Z(:,2)),Z(:,1).*cos(Z(:,2)),'*')%ekf滤波Qk=diag([0;dax;0;day])^2;Rk=diag([dr;dafa])^2;Xk=zeros(4,1);Pk=100*eye(4);X_est=X;fork=1:lenFt=JacobianF(X(k,:),kx,ky,g);Hk=JacobianH(X(k,:));fX=fff(X(k,:),kx,ky,g,Ts);hfX=hhh(fX,Ts);[Xk,Pk,Kk]=ekf(eye(4)+Ft*Ts,Qk,fX,Pk,Hk,Rk,Z(k,:)'-hfX);X_est(k,:)=Xk';end二、扩展Kalman滤波(EKF)算法figure(1),plot(X_est(:,1),X_est(:,3),'+r')xlabel('X');ylabel('Y');title('ekfsimulation');legend('real','measurement','ekfestimated');%%%%%%%%%%%%%%%%%%%%子程序%%%%%%%%%%%%%%%%%%%functionF=JacobianF(X,kx,ky,g)%系统状态雅可比函数vx=X(2);vy=X(4);F=zeros(4,4);F(1,F(2,2)=-2*kx*vx;F(3,4)=1;F(4,4)=2*ky*vy;2)=1;二、扩展Kalman滤波(EKF)算法functionH=JacobianH(X)%量测雅可比函数x=X(1);y=X(3);H=zeros(2,4);r=sqrt(x^2+y^2);H(1,1)=1/r;H(1,3)=1/r;xy2=1+(x/y)^2;H(2,1)=1/xy2*1/y;H(2,3)=1/xy2*x*(-1/y^2);functionfX=fff(X,kx,ky,g,Ts)%系统状态非线性函数x=X(1);vx=X(2);y=X(3);vy=X(4);x1=x+vx*Ts;vx1=vx+(-kx*vx^2)*Ts;y1=y+vy*Ts;vy1=vy+(ky*vy^2-g)*Ts;fX=[x1;vx1;y1;vy1];二、扩展Kalman滤波(EKF)算法functionhfX=hhh(fX,Ts)%量测非线性函数x=fX(1);y=fX(3);r=sqrt(x^2+y^2);a=atan(x/y);hfX=[r;a];function[Xk,Pk,Kk]=ekf(Phikk_1,Qk,fXk_1,Pk_1,Hk,Rk,Zk_hfX)%ekf滤波函数Pkk_1=Phikk_1*Pk_1*Phikk_1'+Qk;Pxz=Pkk_1*Hk';Pzz=Hk*Pxz+Rk;Kk=Pxz*Pzz^-1;Xk=fXk_1+Kk*Zk_hfX;Pk=Pkk_1-Kk*Pzz*Kk';二、扩展Kalman滤波(EKF)算法图2仿真结果020406080100120140160180200360380400420440460480500520ekfsimulationXYrealmeasurementekfestimated三、无迹卡尔曼滤波算法(UKF)为了改善对非线性问题进行滤波的效果，Julier等人提出了采用基于unscented变换的UKF方法UKF不是和EKF一样去近似非线性模型，而是对后验概率密度进行近似来得到次优的滤波算法。UKF算法的核心是UT变换，UT是一种计算非线性变换中的随机变量的统计特征的新方法，是UKF的基础。三、无迹卡尔曼滤波算法(UKF)假设n维随机向量,x通过非线性函数y=f(x)变换后得到n维的随机变量y。通过UT变换可以以较高的精度和较低的计算复杂度求得y的均值和方差。UT的具体过程可描述如下:(1)计算2n+1个Sigma点及其权值:),(:xPxNxyxP)6.........(..........2,...,2,1),(2/1)1()/()/()5......(2,...,1)1(,...,2,1)1(,2000ninwwnwnwnniPnxXniPnxXxXcimicmxixi三、无迹卡尔曼滤波算法(UKF)式中:,决定Sigma点的散布程度，通常取一小的正值,k通常取0;用来描述x的分布信息了高斯情况下，的最优值为2);为矩阵平方根第i列；为均值的权值，为方差的权值。(2)计算Sigma点通过非线性函数的传播结果:从而可知：nkn)(2xPn)(miwciw)(f)7..(..........2,...,1,0),(niXfYii三、无迹卡尔曼滤波算法(UKF)由于x的均值和方差都精确到二阶，计算得到y的均值和方差也精确到二阶，比线性化模型精度更高。在卡尔曼框架内应用UT技术就得到了UKF算法。)10....(..........))(()9......(..........))(()8(........................................202020TiinicixyTiiniciyinimiyYxYwPyYyYwPYwy三、无迹卡尔曼滤波算法(UKF)UKF是用确定的采样来近似状态的后验PDF,可以有效解决由系统非线性的加剧而引起的滤波发散问题，但UKF仍是用高斯分布来逼近系统状态的后验概率密度，所以在系统状态的后验概率密度是非高斯的情况下，滤波结果将有极大的误差。三、无迹卡尔曼滤波算法(UKF)Matlab程序：%---------------------------------------functionUKFmain%------------------清屏----------------closeall;clearall;clc;tic;globalQfn;%定义全局变量%------------------初始化--------------stater0=[220;1;55;-0.5];%标准系统初值state0=[200;1.3;50;-0.3];%测量状态初值%--------系统滤波初始化p=[0.005000;00.00500;000.0050;0000.005];%状态误差协方差初值n=4;T=3;Qf=[T^2/20;0T;T^2/20;0T];%--------------------------------------stater=stater0;state=state0;xc=state;staterout=[];stateout=[];xcout=[];errorout=[];tout=[];t0=1;h=1;tf=1000;%仿真时间设置三、无迹卡尔曼滤波算法(UKF)%---------------滤波算法----------------fort=t0:h:tf[state,stater,yc]=track(state,stater);%轨迹发生器：标准轨迹和输出[xc,p]=UKFfiter(@systemfun,@measurefun,xc,yc,p);error=xc-stater;%滤波处理后的误差staterout=[staterout,stater];stateout=[stateout,state];errorout=[errorout,error];xcout=[xcout,xc];tout=[tout,t];end%---------------状态信息图像---------------figure;plot(tout,xcout(1,:),'r',tout,staterout(1,:),'g',...tout,stateout(1,:),'black');legend('滤