拓展卡尔曼滤波

南京航空航天大学随机信号小论文题目扩展卡尔曼滤波学生姓名梅晟学号SX1504059学院电子信息工程学院专业通信与信息系统扩展卡尔曼滤波一、引言20世纪60年代，在航空航天工程突飞猛进而电子计算机又方兴未艾之时，卡尔曼发表了论文《ANewApproachtoLinearFilteringandPredictionProblems》（一种关于线性滤波与预测问题的新方法），这让卡尔曼滤波成为了时域内有效的滤波方法，从此各种基于卡尔曼滤波的方法横空出世，在目标跟踪、故障诊断、计量经济学、惯导系统等方面得到了长足的发展。二、卡尔曼滤波器卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器),它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑便使用了这种滤波器。目前,卡尔曼滤波已经有很多不同的实现。卡尔曼最初提出的形式现在一般称为简单卡尔曼滤波器。除此以外，还有施密特扩展滤波器、信息滤波器以及很多Bierman,Thornton开发的平方根滤波器的变种。也许最常见的卡尔曼滤波器是锁相环，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。三、扩展卡尔曼滤波器3.1被估计的过程信号卡尔曼最初提出的滤波理论只适用于线性系统,Bucy，Sunahara等人提出并研究了扩展卡尔曼滤波(ExtendedKalmanFilter，简称EKF)，将卡尔曼滤波理论进一步应用到非线性领域。EKF的基本思想是将非线性系统线性化，然后进行卡尔曼滤波，因此EKF是一种次优滤波。同泰勒级数类似，面对非线性关系时，我们可以通过求过程和量测方程的偏导来线性化并计算当前估计。假设过程具有状态向量𝑥∈ℜ𝑛，其状态方程为非线性随机差分方程的形式。𝑥𝑘=𝑓(𝑥𝑘−1,𝑢𝑘−1,𝑤𝑘−1)(1.1)观测变量𝑧∈ℜ𝑚为：𝑧𝑘=ℎ(𝑥𝑘,𝑣𝑘)(1.2)随机变量𝑤𝑘和𝑣𝑘代表过程激励噪声和观测噪声。它们为相互独立，服从正态分布的白色噪声：p(w)∼N(0,Q),p(v)∼N(0,R).实际系统中，过程激励噪声协方差矩阵Q和观测噪声协方差矩阵R可能会随每次迭代计算而变化。但在这我们假设它们是常数。差分方程式(1.1)中的非线性函数f将过去k−1时刻状态与现在k时刻状态联系起来。量测方程(1.2)中的驱动函数𝑢𝑘和零均值过程噪声𝑤𝑘是它的参数。非线性函数h反映了状态变量𝑥𝑘和观测变量𝑧𝑘的关系。实际中我们显然不知道每一时刻噪声𝑤𝑘和𝑣𝑘各自的值。但是，我们可以将它们假设为零，从而估计状态向量和观测向量为：𝑥̃𝑘=𝑓(𝑥̂𝑘−1,𝑢𝑘−1,0)(1.3)和𝑧̃𝑘=ℎ(𝑥̃𝑘,0)(1.4)其中，𝑥̂𝑘是过程相对前一时刻k的后验估计。有一点非常重要，那就是扩展卡尔曼滤波器的一个基本缺陷：离散随机变量的分布（或连续随机变量的密度）在经过非线性系统转化后不再是正态的了。扩展卡尔曼滤波器其实就是一个通过线性化而达到渐进最优贝叶斯决策的特殊状态估计器。3.2滤波器的计算原型为了估计一个具有非线性差分和量测关系的过程，我们先给出式(1.3)和式(1.4)的一个新的线性化表示：𝑥𝑘≈𝑥̃𝑘+𝐴(𝑥𝑘−1−𝑥̂𝑘−1)+𝑊𝑤𝑘−1(1.5)𝑧𝑘≈𝑧̃𝑘+𝐻(𝑥𝑘−𝑥̃𝑘)+𝑉𝑣𝑘(1.6)其中，•𝑥𝑘和𝑧𝑘是状态向量和观测向量的真值，•𝑥̃𝑘和𝑧̃𝑘来自1.3式和1.4式，是状态向量和观测向量的近似值，•𝑥̂𝑘是k时刻状态向量的后验估计，•随机变量𝑤𝑘和𝑣𝑘表示过程激励噪声和观测噪声。•A是f对x的偏导的雅可比矩阵：𝐴[𝑖,𝑗]=𝜕𝑓[𝑖]𝜕𝑥[𝑗](𝑥̂𝑘−1,𝑢𝑘−1,0)•W是f对w的偏导的雅可比矩阵：𝑊[𝑖,𝑗]=𝜕𝑓[𝑖]𝜕𝑤[𝑗](𝑥̂𝑘−1,𝑢𝑘−1,0)•H是h对x的偏导的雅可比矩阵：𝐻[𝑖,𝑗]=𝜕ℎ[𝑖]𝜕𝑥[𝑗](𝑥̃𝑘,0)•V是h对v的偏导的雅可比矩阵：𝑉[𝑖,𝑗]=𝜕ℎ[𝑖]𝜕𝑣[𝑗](𝑥̃𝑘,0)现在我们定义一个新的预测误差的表达式：𝑒̃𝑥𝑘≡𝑥𝑘−𝑥̃𝑘(1.7)和观测变量的残余，𝑒̃𝑧𝑘≡𝑧𝑘−𝑧̃𝑘(1.8)但实际中无法获得(1.7)式中的𝑥𝑘,它是状态向量的真值，也就是要估计的对象。同样，(1.8)式中的𝑧𝑘也是无法获取的，它是用来估计𝑥𝑘的观测向量的真值。由(1.7)式和(1.8)式我们可以写出误差过程的表达式：𝑒̃𝑥𝑘≈𝐴(𝑥𝑘−1−𝑥̂𝑘−1)+𝜖𝑘(1.9)𝑒̃𝑧𝑘≈𝐻𝑒̃𝑥𝑘+𝜂𝑘(1.10)𝜖𝑘和𝜂𝑘代表具有零均值和协方差矩阵WQWT和VRVT的独立随机变量，Q和R分别为过程激励噪声协方差矩阵和观测噪声协方差矩阵。在此我们利用(1.8)式中的观测残余真值𝑒̃𝑧𝑘去估计(1.9)式中的预测误差𝑒̃𝑥𝑘，估计结果记为𝑒̂𝑘，结合(1.7)式可以获得初始非线性过程的后验状态估计：𝑥̂𝑘=𝑥̃𝑘+𝑒̂𝑘(1.11)(1.9)式和(1.10)式中的随机变量具有如下概率分布:𝑝(𝑒̃𝑥𝑘)~𝑁(0,𝐸[𝑒̃𝑥𝑘𝑒̃𝑥𝑘𝑇])𝑝(𝜖𝑘)~𝑁(0,𝑊𝑄𝑘𝑊𝑇)𝑝(𝜂𝑘)~𝑁(0,𝑉𝑅𝑘𝑉𝑇)令𝑒̂𝑘的估计值为零，由以上近似，可以写出估计𝑒̂𝑘的卡尔曼滤波器表达式：𝑒̂𝑘=𝐾𝑘𝑒̃𝑧𝑘(1.12)将(1.8)式和(1.12)式代入(1.11)式，得到：𝑥̂𝑘=𝑥̃𝑘+𝐾𝑘𝑒̃𝑧𝑘=𝑥̃𝑘+𝐾𝑘(𝑧𝑘−𝑧̃𝑘)(1.13)3.3拓展卡尔曼滤波总结扩展卡尔曼滤波器的一个重要特性是卡尔曼增益Kk的表达式中的雅可比矩阵Hk能够正确地传递或“加权”观测信息中的有用部分。例如，如果通过h观测变量𝑧𝑘和状态变量没有一一对应的关系，雅可比矩阵Hk便通过改变卡尔曼增益从而使得残余𝑧𝑘−ℎ(𝑥̂𝑘−,0)中真正作用于状态变量的部分被加权。当然，如果整个观测中观测变量𝑧𝑘和状态变量通过h都没有一个一一对应的关系，那么滤波器很快就会发散。这种情况下过程是不可观测的。拓展卡尔曼滤波器基本运行流程图如下：四、Matlab仿真附程序如下:clc;closeall;clearall;Xint_v=[1;0;0;0;0];wk=[10000];vk=[10000];forii=1:1:length(Xint_v)Ap(ii)=Xint_v(ii)*2;W(ii)=0;H(ii)=-sin(Xint_v(ii));V(ii)=0;Wk(ii)=0;endUk=randn(1,200);Qu=cov(Uk);Vk=randn(1,200);Qv=cov(Vk);C=[10000];n=100;[YYXX]=EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);forit=1:1:length(XX)MSE(it)=YY(it)-XX(it);endtt=1:1:length(XX);figure(1);subplot(211);plot(XX);title('ORIGINALSIGNAL');subplot(212);plot(YY);title('ESTIMATEDSIGNAL');figure(2);plot(tt,XX,tt,YY);title('Combinedplot');legend('original','estimated');figure(3);plot(MSE.^2);title('Meansquareerror');%--------------------------------------------------------------------%--------------------------------------------------------------------function[YY,XX]=EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);Ap(2,:)=0;forii=1:1:length(Ap)-1Ap(ii+1,ii)=1;endinx=1;UUk=[Uk(inx);0;0;0;0];PPk=(Xint_v*Xint_v');VVk=[Vk(inx);0;0;0;0];Qv=V*V';forii=1:1:length(Xint_v)XKk(ii,1)=Xint_v(ii)^2;%FIRSTSTEPendPPk=Ap*PPk*Ap';%SECONDSTEPKk=PPk*C'*inv((C*PPk*C')+(V*Qv*V'));%THIRDSTEPforii=1:1:length(Xint_v)XUPK(ii,1)=XKk(ii)^2+UUk(ii);%UPPEREQUATIONS.Zk(ii,1)=cos(XUPK(ii))+VVk(ii);%UPPEREQUATIONS.endforii=1:1:length(XKk)XBARk(ii,1)=XKk(ii)+Kk(ii)*(Zk(ii)-(cos(XKk(ii))));%FOURTHSTEPendII=eye(5,5);Pk=(II-Kk*C)*PPk;%FIFTHSTEP%--------------------------------------------------------------------%--------------------------------------------------------------------forii=1:1:nUUk=[Uk(ii+1);0;0;0;0];PPk=XBARk*XBARk';VVk=[Vk(ii+1);0;0;0;0];XKk=exp(-XBARk);%FIRSTSTEPPPkM=Ap*PPk*Ap';%SECONDSTEPKk=PPkM*C'*inv((C*PPkM*C')+(V*Qv*V'));%THIRDSTEPfornn=1:1:length(XBARk)XUPK(nn)=exp(-XKk(nn))+UUk(nn);%UPPEREQUATIONS.Zk(nn)=cos(XUPK(nn))+VVk(nn);%UPPEREQUATIONS.endforin=1:1:length(XUPK)XNEW(in)=XBARk(in)+Kk(in)*(Zk(in)-cos(XBARk(in)));%FOURTHSTEPendII=eye(5,5);Pk=(II-Kk*C)*PPkM;%FIFTHSTEPXBARk=XNEW;OUTX(ii)=XBARk(1,1);OUTY(ii)=Zk(1,1);endYY=OUTY;XX=OUTX;