卡尔曼滤波的学习

1.2Kalman滤波理论的基础在估计问题中，长考虑如下随机线性离散系统模型X二①x+rwkk,k-1k-1k,k-1k-1Z二HX+Vk kkkX是系统的n维状态向量，Z是系统的m维观察向量。kk根据状态向量和观察向量在时间上存在的不同对应关系，我们可以把估计问题分为滤波、预测和平滑，以上式所描述的随机线性离散系统为例，设£表示根据j时刻和j以前时刻的k,j观察值，对k时刻状态X做出的某种估计，则按照k和j的不同对应关系，叙述如下：k当k=j时，对X的估计称为滤波，即依据过去直至现在的观察测量来估计现在的k,j状态。相应地，称丘为X的最有滤波估计值，简记为X。这类估计主要用于随k,jk k机系统的实时控制。当k>j时对X的估计称为预测或外推，即依据过去直至现在的观察测量来预测未k,j来的状态，并把丘称为X的最优预测估计值。这类估计广泛应用于对系统未来k,j k状态的预测和实时控制。当kvj时对X的估计称为平滑或内插，即依据过去直至现在的观察测量去估计过k,j去的历史状态，并称X为X的最优平滑估计值。这类估计广泛应用于通过分析k,j k实验或试验数据，对系统进行评估。在预测、滤波和平滑三类状态估计问题中预测是滤波的基础，滤波是平滑的基础。最早的估计方法是高斯提出的最小二乘法，最小二乘法没有考虑到被估参数和观测数据的统计特性，因此这种方法不是最优估计方法。Wiener滤波器采用频域设计法，运算复杂，解析求解困难，整批数据处理要求存储空间大，造成其适用范围及其有限，仅适用于一维平稳随机过程信号滤波。Kalman滤波采用了和Wiener滤波相同的估计准则，二者的基本原理一致，但是kalman滤波是一种时域滤波方法，采用状态空间方法描述系统，算法采用递推形式，数据存储量小，不仅可以处理平稳随机过程，也可以处理多维和非平稳随机过程。关于系统过程噪声和观测噪声的统计特性如下：kkj'E[W]=0,E「WWt]=Q5kkjk kj kkkjE[V]=0,E「VVt]=R5kkjk kjE「WVt]=0kj如果被估计状态X和对X的观测量Z满足上式约束，系统过程噪声W和观测噪声k k k kV满足上式的假设，系统过程噪声方差阵Q非负定，系统观测噪声方差阵R正定，k时刻kkk的观测为Z，则X的估计X可按下述方程求解:kkk状态一步预测:X二①Xk,k—1 k,k—1k—1状态估计X=X+krz—hX~k k—1kkkk,k—1滤波增益矩阵K=PHtrHPHt+R1—1kk,k—1kkk,k—1kk一步预测误差方差阵P=OP①T+rQFtk,k—1k,k—1k—1k,k—1k,k—1k—1k,k—1估计误差方差阵P=[i—KH]p[i—KH卜+KRKtk kkk,k—1 kk kkk其中K=PHTR—1kkkk其中P=[I—KH]pk kkk,k—1P-1=P-1+HtR-iHk k,k—1 kkkKalman滤波算法的特点:（1） 由于Kalman滤波算法将被估计的信号看作在白噪声作用下一个随机线性系统的输出，并且其输入输出关系是由状态方程和输出方程在时间域内给出的，因此这种滤波方法不仅适用于平稳序列的滤波，而且特别适用于非平稳马尔科夫序列或高斯-马尔科夫序列的滤波，因此其应用范围是十分广泛的。（2） 由于Kalman滤波的基本方程时间域内的递推形式，其计算过程是一个不断地“预测-修正”过程，在求解是不要求存储大量的数据，并且一旦观测到了新的数据，随时可以算的新的滤波值，因此这种滤波方法非常便于实时处理，计算机实现。（3） 由于滤波器的增益矩阵于观测无关，因此它可离线算出，从而可以减少实时在线计算量；在求滤波器增益矩阵K时要求一个矩阵的逆，既要计算k「HPHt+R ，它的阶数之取决于观测方程的维数m而m通常是最kk,k—1kk小的这样，上面的求逆运算是比较方便的；另外在求解滤波器增益的过程中随时可以算得滤波器的精度指标P，其对角线上的元就是滤波误差向量各分k量的方差。增益矩阵K与初始方差P,系统噪声方差阵Q以及观测噪声方差阵R之k 0 k-1 k间具有如下关系：由基本滤波方程可见，当R增大时，K就变小，噪声变kk大虑波增益就应取小。如果P变小，Qk-1变小，因为P小表示初始估计较好，Q变小表示系统噪声变小，于是增益矩阵应变小以便较小的修正。k-1扩展kalman滤波在车辆GPS/dr组合定位系统中的应用GPS/DR组合系统状态方程的建立取组合定位系统的状态变量为X二[x,v,a,x,v,a]T，其中x，x分别为车辆东向和北eeennnena分别为车辆东向和北向n向的位置分量；v，v分别为车辆东向和北向的速度分量；aa分别为车辆东向和北向nene的加速度分量。则得到组合定位系统连续的状态方程为：X(t)=AX(t)+U+W(t)TTaaenTTaaen分别为车辆东向和北向机动加速度变01000000010000-011000000aTT e3aeU=ae，W(t)=ae000010000000010011300000aaTT nnanan33分别为(0Q2)(0Q2)的高斯白噪声;aaen化率的相关时间常数；aa分别为车辆东向和北向机动加速度分量的“当前”均值。en设采样周期为T,将系统连续的状态方程离散化，得到系统离散的状态方程为X二①X+U+W式4.79kk,k-1k,k-1kk式中，X=[xvaxva]Tk e(k)e(k) e(k) n(k) n(k)n(k)①二diag[^，①]k,k-1e(k,k-1) n(k,k-1)11令a二二 ，a二，则① ，①为eT nT e(k,k-1) n(k,k-1)

1T①=01e(k,k-1)001T①=01n(k,k1T①=01e(k,k-1)001T①=01n(k,k—1)00ee(1—e—aeT)a—1ee—aeTa—2(—1+aT+e—anT)nn(1—e—anT)a—1ne—aTnuuu其中，—T+0.5其中，—T+0.5aT2+(1—e—aeT)a—1eea—1aeea—1aeea—1annu=—T+0.5aT2+(1—ea—1aeea—1ann1 e eT—(1—e—aeT)a—1eu=(1—e—aeT)a3e—T+0.5aT2+(1—e—anT)a—1nnT—(1—e—anT)a—1nu=(1—e—anT)a6n式4.79就是所建立的GPS/DR组合定位系统的状态方程。GPS/DR组合系统观测方程的建立将GPS输出的东向位置信息e北向位置信息n，角速率陀螺的输出«以及里程计(或obsobs车速计)在一个采样周期内输出的距离s作为观测量,；里程计的刻度系数取为1•观测量和状态变量之间的关系如下e=x+vobse1n=x+vobsn2sva—vas—n~~e e―n+£v2+v2 3en于是系统连续的观测方程为：

eobsnobssxexeobsnobssxexn1v2££vv分别为GPS接收机输出的东向位置和北向位置的观测噪声，可近似为(0,Q2)(0,^2)的1211高斯白噪声；£为陀螺的漂移，近似为(0,b2)的高斯白噪声；£为里程计的观测噪声，近s似为(0,G2)的高斯白噪声。s将观测方程离散化，得到系统离散的观测方程为Z=h[x]+V (4.86)式中，h[X]=nobs(k)kx式中，h[X]=nobs(k)kxve(k)xn(k)a_vak obs(k)n(k)e(k) e(k)n(k)v2 +v2e(k) n(k)T\：v2 +v2e(k) n(k)v1(k)v，V= 2(k)k£W(k)£-s(k)-从式(4・86)知，观测方程是非线性的。采用扩展Kalman滤波进行线性化，将h[xJ在预测值乂丘‘-I处按泰勒级数展开并忽略二次以上的高次项，得Z二h「X+Hx-X+VZ二h「X+Hx-X+Vkk,k_1kk k,k_1k化简得4.87)X-HX4.88)「100000-dh[X]000100H= — =k dX xk=X0hh0hhk k,k_1k12340h00h0-56a v 一2vva_av2k,k-1kk,k-1其中h=―n(k,k_1)_e(k,k_1) e(k,k_1)_n(k,k_1)_e(k,k_1) n(k,k_1)_n(k,k_1)12v2 +v2 2n(k,k_1) e(k,k_1)h二 n(k,k-1) TOC\o"1-5"\h\z2 V2 +V2n(k,k-1) e(k,k-1)aV+2vva一aV2e(k,k—1)e(k,k—1) e(k,k—1)n(k,k—1)n(k,k—1) e(k,k—1)n(k,k—1)V2 +V2n(k,k—1) e(k,k—1)-ve(k,k—1)V2 +V2n(k,k-1) e(k,k-1)TVe(k,k-1)IV2 +V2n(k,k—1) e(k,k—1)TVn(kk1)rV2 +V2e(k,k—1) n(k,k—1)(4.88)就是所建立的GPS/DR系统线性离散的观测方程。根据扩展Kalman滤波递推方程和所建立的GPS/DR组合定位系统的状态方程式4.79和4.88式，可得系统的递推滤波方程如下二X式，可得系统的递推滤波方程如下二Xk,k—1+K「Z—h[X ]]k k,k—1」Xk—1"kk—1Xk—1+Uk—1 式4・9°K=P Ht[HP Ht+R]-1k k,k—1kkk,k—1kkP二①P①t+Qk,k—1 k,k—1k—1k,k—1 k—1P二[I—KH]Pk kkk,k—1递推方程中的①丘,k—1、U的表达式可从式(4.81)获得，递推方程中的①丘,k—1k k k的观测噪声的协方差有关，Qk如下Q=E[WWt]=diag[26aQ,26aQ]k kk aee(k) ann(k)en「qqq「qqqe11e12e13n11n12n13Q=qqq，Q=qqqe(k)e21e22e23n(k)n21n22n23qqq」qqq」e31e32e33n31n32n33其中q =0.5a-5(1—e-2«eT+2aT+2a江33-1—2a2T2—4aTe-q)e11 e e e e eq=q =0.5a-4(1+e—2aeT—2e-aeT+2aTe-aT—2aT+a2T2)e12 e21 e e eeq=q =0.5a—3(1—e—2aeT—2aTe—aeT)e13 e31 e eTOC\o"1-5"\h\zq二q二0.5a-2(1+e-2«eT—2e-aT)e23 e32 eq 二0.5a-3(-3—e-2aeT+4e-aeT+2aT)\o"CurrentDocument"e22 e eq 二0.5a-1(1-e-2aeT)\o"CurrentDocument"e33 e e式中QQ都是对称矩阵，Q中的元素表达式和Q中的元素表达式相似，将Qe(k)n(k) n(k) e(k) e(k)中的各元素表达式中的a用a来代替，即可相应地得到Q中的元素表达式。en n(k)若把加速度的一步预测看作“当前”加速度的均值，即a=a a=ae(k)e(k,k-1) n(k) n(k-,k1)则式4.90可化简为TOC\o"1-5"\h\z八 八\o"CurrentDocument"x =0Xk,k-1 1(k,k-1)k-10 =diag[0(T),0(T)]1(k,k-1) 1e 1n1T①(T)=O(T)=011e 1n00值得注意的是，在实际的使用中为了建立系统精确的数学模型，必须事先对系统GPS接收机的测量误差、陀螺漂移以及里程计的测量误差特性有深入的认识，这是一个相当复杂的过程且很重要的过程。否则，如果建立的系统模型和实际系统相差很大，则可能导致kalman滤波效果很差，组合后的系统性能反而变坏。自适应滤波GPS/INS组合导航系统自适应滤波全球定位系统GPS和惯性导航系统INS都是目前世界上应用最为广泛的导航方法之一。利用二者较强的非相似性和互补性将二者组合起来，可以取长补短，充分发挥各自的优势，同时克服了GPS易受地形地物遮挡而导致定位中断和INS定位误差随时间而积累的缺陷。在GPS/INS组合导航系统中，由于系统本身元器件的不稳定性以及外部应用环境不确定因素的影响，给系统噪声和观测噪声统计特性的准确描述带来困难,如果采用常规Kalman滤波器，在实际应用中不能保证收敛性和稳定性。在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！卡尔曼全名RudolfEmilKalman,匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953,1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和I960年发表的论文《ANewApproachtoLinearFilteringandPredictionProblems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下载：/~welch/kalman/media/pdf/Kalman1960.pdf简单来说，卡尔曼滤波器是一个“optimalrecursivedataprocessingalgorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。2．卡尔曼滤波器的介绍IntroductiontotheKalmanFilter)为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（WhiteGaussianNoise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（GaussianDistribution）o另外，我们在房间里放一个温度计，但是这个温度计也不准确的,测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为KgA2=5A2/(5A2+4A2)，所以Kg=0.78,我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小(比较相信温度计)，所以估算出的最优温度值偏向温度计的值。现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值(24.56度)的偏差。算法如下:((1-Kg)*5A2)A0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差(对应于上面的3)。就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益(KalmanGain)。他可以随不同的时刻而改变他自己的值，是不是很神奇！下面就要言归正传，讨论真正工程系统上的卡尔曼。3.卡尔曼滤波器算法(TheKalmanFilterAlgorithm)在这一部分，我们就来描述源于DrKalman的卡尔曼滤波器。下面的描述，会涉及一些基本的概念知识，包括概率(Probability)，随即变量(RandomVariable)，高斯或正态分配(GaussianDistribution)还有State-spaceModel等等。但对于卡尔曼滤波器的详细证明，这里不能一一描述。首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程(LinearStochasticDifferenceequation)来描述：X(k)=AX(k-1)+BU(k)+W(k)再加上系统的测量值：Z(k)=HX(k)+V(k)上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵°Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(WhiteGaussianNoise)，他们的covariance分别是Q,R(这里我们假设他们不随系统状态变化而变化)。对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances来估算系统的最优化输出(类似上一节那个温度的例子)。首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k,根据系统的模型，可以基于系统的上一状态而预测出现在状态：X(k|k-1)=AX(k-1|k-1)+BU(k)………..(1)式⑴中，X(klk-l)是利用上一状态预测的结果，X(k-llk-l)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。到现在为止，我们的系统结果已经更新了，可是，对应于X(klk-l)的covariance还没更新。我们用P表示covariance：P(klk-l)=AP(k-llk-1)A'+Q………(2)式(2)中，P(klk-1)是X(klk-1)对应的covariance，P(k-llk-l)是X(k-1lk-1)对应的covariance，A表示A的转置矩阵，Q是系统过程的covariance。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(klk)：X(klk)=X(klk-1)+Kg(k)(Z(k)-HX(klk-1))………(3)其中Kg为卡尔曼增益(KalmanGain)：Kg(k)=P(klk-1)H'/(HP(k|k-1)H'+R)………(4)到现在为止，我们已经得到了k状态下最优的估算值X(klk)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(klk)的covariance：P(klk)=(I-Kg(k)H)P(klk-1)………(5)其中I为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(klk)就是式子(2)的P(k-1lk-1)。这样，算法就可以自回归的运算下去。卡尔曼滤波器的原理基本描述了，式子1，2，3，4和5就是他的5个基本公式。根据这5个公式，可以很容易的实现计算机的程序。下面，我会用程序举一个实际运行的例子。。。4．简单例子(ASimpleExample)这里我们结合第二第三节，举一个非常简单的例子来说明卡尔曼滤波器的工作过程。所举的例子是进一步描述第二节的例子，