拓展卡尔曼滤波(共8页).docx
精选优质文档-倾情为你奉上 南京航空航天大学 随机信号小论文题 目扩展卡尔曼滤波学生姓名梅晟学 号SX学 院电子信息工程学院专 业通信与信息系统扩展卡尔曼滤波一、引言20世纪60年代，在航空航天工程突飞猛进而电子计算机又方兴未艾之时，卡尔曼发表了论文A New Approach to Linear Filtering and Prediction Problems（一种关于线性滤波与预测问题的新方法），这让卡尔曼滤波成为了时域内有效的滤波方法，从此各种基于卡尔曼滤波的方法横空出世，在目标跟踪、故障诊断、计量经济学、惯导系统等方面得到了长足的发展。二、卡尔曼滤波器卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全及包含噪声的中，估计的状态。卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，对物体位置的观察序列（可能有偏差）预测出物体的位置的及。卡尔曼在NASA研究中心访问时，发现他的方法对于解决的轨道预测很有用，后来阿波罗飞船的导航电脑便使用了这种滤波器。目前,卡尔曼滤波已经有很多不同的实现。卡尔曼最初提出的形式现在一般称为简单卡尔曼滤波器。除此以外，还有施密特扩展滤波器、信息滤波器以及很多Bierman, Thornton 开发的平方根滤波器的变种。也许最常见的卡尔曼滤波器是，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。三、扩展卡尔曼滤波器3.1 被估计的过程信号卡尔曼最初提出的滤波理论只适用于线性系统,Bucy，Sunahara等人提出并研究了扩展卡尔曼滤波(Extended Kalman Filter，简称EKF)，将卡尔曼滤波理论进一步应用到非线性领域。EKF的基本思想是将非线性系统线性化，然后进行卡尔曼滤波，因此EKF是一种次优滤波。同泰勒级数类似，面对非线性关系时，我们可以通过求过程和量测方程的偏导来线性化并计算当前估计。假设过程具有状态向量xRn， 其状态方程为非线性随机差分方程的形式。xk=fxk-1,uk-1,wk-1 (1.1)观测变量zRm为：zk=h(xk,vk) (1.2)随机变量wk和vk代表过程激励噪声和观测噪声。它们为相互独立，服从正态分布的白色噪声：p(w) N(0, Q),p(v) N(0, R).实际系统中，过程激励噪声协方差矩阵 Q 和观测噪声协方差矩阵 R 可能会随每次迭代计算而变化。但在这我们假设它们是常数。差分方程式(1.1)中的非线性函数 f 将过去 k 1 时刻状态与现在 k 时刻状态联系起来。 量测方程(1.2)中的驱动函数 uk 和零均值过程噪声 wk 是它的参数。 非线性函数 h 反映了状态变量 xk 和观测变量 zk 的关系。实际中我们显然不知道每一时刻噪声 wk 和 vk 各自的值。但是，我们可以将它们假设为零，从而估计状态向量和观测向量为：xk=fxk-1,uk-1,0 (1.3)和zk=h(xk,0) (1.4)其中，xk 是过程相对前一时刻 k 的后验估计。有一点非常重要，那就是扩展卡尔曼滤波器的一个基本缺陷：离散随机变量的分布（或连续随机变量的密度）在经过非线性系统转化后不再是正态的了。扩展卡尔曼滤波器其实就是一个通过线性化而达到渐进最优贝叶斯决策的特殊状态估计器。3.2 滤波器的计算原型为了估计一个具有非线性差分和量测关系的过程， 我们先给出式(1.3)和式(1.4)的一个新的线性化表示：xkxk+Axk-1-xk-1+Wwk-1 (1.5)zkzk+Hxk-xk+Vvk (1.6)其中， xk和 zk是状态向量和观测向量的真值， xk和 zk来自1.3式和1.4式，是状态向量和观测向量的近似值， xk是 k 时刻状态向量的后验估计， 随机变量 wk 和 vk表示过程激励噪声和观测噪声。 A 是 f 对 x 的偏导的雅可比矩阵：Ai,j=fixjxk-1,uk-1,0 W 是 f 对 w 的偏导的雅可比矩阵：Wi,j=fiwjxk-1,uk-1,0 H 是 h 对 x 的偏导的雅可比矩阵：Hi,j=hixjxk,0 V 是 h 对 v 的偏导的雅可比矩阵：Vi,j=hivjxk,0现在我们定义一个新的预测误差的表达式：exkxk-xk (1.7)和观测变量的残余，ezkzk-zk (1.8)但实际中无法获得(1.7)式中的xk ,它是状态向量的真值，也就是要估计的对象。同样， (1.8)式中的zk也是无法获取的，它是用来估计xk的观测向量的真值。 由(1.7)式和(1.8)式我们可以写出误差过程的表达式：exkAxk-1-xk-1+k (1.9)ezkHexk+k (1.10)k 和 k 代表具有零均值和协方差矩阵 WQWT 和 VRVT 的独立随机变量， Q 和 R 分别为过程激励噪声协方差矩阵和观测噪声协方差矩阵。在此我们利用(1.8)式中的观测残余真值ezk去估计(1.9)式中的预测误差exk，估计结果记为ek，结合(1.7)式可以获得初始非线性过程的后验状态估计：xk=xk+ek (1.11)(1.9)式和(1.10)式中的随机变量具有如下概率分布:p(exk)N(0,EexkexkT)p(k)N(0,WQkWT)p(k)N(0,VRkVT)令ek的估计值为零，由以上近似，可以写出估计ek 的卡尔曼滤波器表达式：ek=Kkezk (1.12)将(1.8)式和(1.12)式代入(1.11)式，得到：xk=xk+Kkezk=xk+Kk(zk-zk) (1.13)3.3 拓展卡尔曼滤波总结扩展卡尔曼滤波器的一个重要特性是卡尔曼增益 Kk 的表达式中的雅可比矩阵 Hk 能够正确地传递或“加权” 观测信息中的有用部分。 例如， 如果通过 h 观测变量 zk 和状态变量没有一一对应的关系， 雅可比矩阵 Hk 便通过改变卡尔曼增益从而使得残余 zk-h(xk-,0)中真正作用于状态变量的部分被加权。 当然， 如果整个观测中观测变量 zk 和状态变量通过 h 都没有一个一一对应的关系， 那么滤波器很快就会发散。 这种情况下过程是不可观测的。拓展卡尔曼滤波器基本运行流程图如下：四、Matlab仿真附程序如下:clc; close all; clear all;Xint_v = 1; 0; 0; 0; 0;wk = 1 0 0 0 0;vk = 1 0 0 0 0;for ii = 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 = 1 0 0 0 0;n = 100; YY XX = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);for it = 1:1:length(XX) MSE(it) = YY(it) - XX(it);endtt = 1:1:length(XX);figure(1); subplot(211); plot(XX); title('ORIGINAL SIGNAL');subplot(212); plot(YY); title('ESTIMATED SIGNAL');figure(2); plot(tt,XX,tt,YY); title('Combined plot'); legend('original','estimated');figure(3); plot(MSE.2); title('Mean square error');%-%-function YY,XX = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);Ap(2,:) = 0;for ii = 1:1:length(Ap)-1 Ap(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'for ii = 1:1:length(Xint_v)XKk(ii,1) = Xint_v(ii)2; %FIRST STEPendPPk = Ap*PPk*Ap' % SECOND STEPKk = PPk*C'*inv( (C*PPk*C') + (V*Qv*V') ); % THIRD STEPfor ii = 1:1:length(Xint_v)XUPK(ii,1) = XKk(ii)2 + UUk(ii); % UPPER EQUATIONS.Zk(ii,1) = cos(XUPK(ii) + VVk(ii); % UPPER EQUATIONS.endfor ii = 1:1:length(XKk)XBARk(ii,1)=XKk(ii)+ Kk(ii)*(Zk(ii) - (cos(XKk(ii) ; % FOURTH STEPendII = eye(5,5);Pk = ( II - Kk*C)*PPk; % FIFTH STEP %-%-for ii = 1:1:nUUk = Uk(ii+1); 0; 0; 0; 0;PPk = XBARk*XBARk'VVk = Vk(ii+1); 0; 0; 0; 0;XKk = exp(-XBARk); % FIRST STEPPPkM = Ap*PPk*Ap' % SECOND STEPKk = PPkM*C'*inv( (C*PPkM*C') + (V*Qv*V') ); % THIRD STEPfor nn = 1:1:length(XBARk)XUPK(nn) = exp(-XKk(nn) + UUk(nn); % UPPER EQUATIONS.Zk(nn) = cos(XUPK(nn) + VVk(nn); % UPPER EQUATIONS.endfor in = 1:1:length(XUPK)XNEW(in) = XBARk(in)+Kk(in)*(Zk(in) - cos(XBARk(in); % FOURTH STEPendII = eye(5,5);Pk = (II - Kk*C)*PPkM; % FIFTH STEPXBARk = XNEW;OUTX(ii) = XBARk(1,1);OUTY(ii) = Zk(1,1);endYY = OUTY;XX = OUTX;专心-专注-专业
