卡尔曼滤波

代码说明：

提供了kf，ekf，ukf的详细推导过程，从标量推导开始，进而转入矢量推导，非常详细卡尔曼滤波器简介(阎泓著第一步、时间更新29第二步、测量更新“““““““+““44““““42924特殊情况.30第一种情况、先验误差极小...-.----130第二种情况、先验误差极大.30第三种情况、测量噪声极大.…31第三章、标量EKF画,通通画4“““““+44=“++“““++4“4“+“4“““-“++323.1非线性状态模型.323.2模型线性化33.2.1过程噪声项的线性化.333.2.2测量噪声项的线性化...11-343.2.3过程和测量噪声项同时线性化…35324过程的线性化…0353.25测量的线性化…363.3EKF滤波器…1373.31应用卡尔曼滤波器.3733,2计算先验均方差373.33计算后验均方差373.3.4计算k值4a“44444“;4444454a44“44444=424444441“如44444;44444“44.45“#4444444a444444443833.5k值为最优时的后验均方差3834算法39第一步、时间更新………9第二步、测量更新393.5EKF的缺陷44“==++++4=++44日+“44=“““+440第四章、矢量EKF4141非线性矢量状态模型4142矢量模型线性化单“““·***“““***“““““***“““***4““-***4““*“→“““*→*-““““““*“““*+4““→*“·““·““““*4242.1矢量泛函的泰勒展开42.2过程噪声项的线性化424.2.3测量噪声项的线性化.→“““#+4+“44“““-4+44→“““4“4+-““+43424过程和测量噪声项同时线性化4442.5过程的线性化4“““4““*“4““*→““*+“4“““““““*4“““4“““++4““44“““4“44““““七426测量的线性化“““““·+““““*““““+“““““““+4“““““““+4“““→·“““+“4543矢量EKF滤波器面面面面46画面和面面,43.1应用矢量卡尔曼滤波器44““++“44“““*44“““++444““4+444“+“44““““+444643.2计算先验均方差4643.3计算后验均方差4““+44““““44““““+→4““““+4““““4“44““““.47434计算k值47435k值为最优时的后验均方差4845算法“““+““““*“““““+…““““*“+44““48第一步、时间更新.…49第3页(共77页)卡尔曼滤波器简介(阎泓著第二步、测量更新““4--““44-4494.4特殊情况.““““4444“画画新通画通49第一种情况、先验误差极小.画画,画画画园画画,画画画面请通.50第二种情况、先验误差极大….----50第三种情况、测量噪声极大44“““+44““=++“44“““+444““4+“44““44+50第五章、标量无迹变换UT5251无迹变换的任务5252真值“““““++“++4“4“““+4“++4“““““+““+“““““525.3无迹测试点1101453.1标量的无迹测试点………154532无迹权重系数翻国口道55533统计性质公式…5554测试点的无迹变换.565.4.1从测试点得到后验期待值.画画通通画画山通画画新56542从测试点得到后验方差“““+4“++“4“++““平““上“““4““平中“+““““平“4+“=575.5讨论品aB444a日日+44日4日日“4日a4日+a日本“日日日和本上日和4日““458第六章矢量无迹变换UT4“““4“44“““4++44“““4+““4+2+“++“4“++4=“++“““2++““““++““4+““““++5961矢量微分回顾5961.1计算真值会用到的恒等式1962矢量无迹变换的任务中本““丰二“中““6063真值6163无迹测试点63.1矢量的无迹测试点画面通自品面画画面自自通国画日画面国通画日通山国国画山山面通画山山丽右日日画画画画画山63632无迹权重系数64633UT变换下的对称性64测试点的无迹变换6564.1几个恒等式…65642从测试点得到后验期待值.…---1----66642从测试点得到后验协方差.6765讨论68第七章、无迹滤波器UKF11116971高维非线性问题.069711标量特例画画画画画画新画画画画画画““*#“““““44“…4“““““4““+““→““““44““47072无迹滤波器面,面面面面面面面“面画70721无迹测试点““*4“““““44““+44““““*44“““++444“““4““+“44“““““722无迹权重系数通画画通画画通通画画通山请画画画画画画出画请画画副。723先验估计画画·画‘画4““+44““““44““““+→4““““+““““+“444““““+472724应用卡尔曼滤波器737.2.5计算后验均方差…737.2.6计算k值…444““+44“““*447473算法75第4页(共77页)卡尔曼滤波器简介(阎泓著第零步、初始化..-75第一步、时间更新175第二步、测量更新画画,画画画园画画,画画画面请通176第5页(共77页)卡尔曼滤波器简介(阎泓著第一章、标量线性系统实际工作中的线性系统很少有标量的,但是标量的卡尔曼滤波器的理论推导比较直观、易于理解,因此作为学习的切入点比较合适首先必须清楚地陈述卡尔曼滤波器要解决的问题。1.1卡尔曼问题在离散时间中,一个标量线性系统的状态演化常常可以表述为下面的随机差分方程式:x=ax,+bu其中t为时间。x,是一个标量随机变量,代表t时刻系统的内禀状态。a和b为常标量。u,为t-1时刻的输入,也是一个标量。111信号流程图上面的(1)式也可以用下面的信号流程图表示u-1)X()Ibax(t-1)直线表示信号的传送,箭头代表传送的方向。流程图中的图标有三种,第一种方框图标代表时间延迟,见下图x(t)TX(t-1)第二种方框图标代表乘法(增益),见下图第6页(共77页)卡尔曼滤波器简介(阎泓著aax第三种圆形图标代表加法(混合),见下图a-b+CbG这些图标可以按照有意义的方式组合起来,描述一个差分方程。必须指出,这些图标并不局限于标量情形,而且适用于矢量情形,譬如x为一个矢量,而a和b可以为矩阵。112加入白噪声假设在这个线性过程中有一个噪声项v鬟x2=ax21+bu-1+W1-1则此方程式可以用下面的信号流程图表示w(t=1)u(-1)中+baX(-1)假定这个噪声ν是一个高斯白噪声,它满足3N(9),(Q20)〈ww)=0(≠)3在本文采用物理学中常用的记号,(x)=E(x)表示x的期待值第7页(共77页)卡尔曼滤波器简介(阎泓著此外假定w与u.没有关联,也即113加入可测量假设系统的状态量x是不可以直接测量的。可以测量的是另外一个量z,称为可测量。可测量z依赖于系统的状态量x和一个激励倍数h,见下式。hx. +v(5)在实际工作中h可能会随着时间而变化,但在这里假定为常数,为常标量。此时流程图如下。wt-1)u(t-1)+b±2(ax(t-1)测量过程本身带有一个噪声ν,影响了测量的准确度。同样我们假定ν是一个白噪声(,R)(R≥0)(")≥=0(s≠)此外假定ν与w和u都没有关联,也即()=v)=0(s1)114卡尔曼问题陈述现在要考虑的是如何从可观测量z;的观测数据中得出x的最优估计值,把噪声w和v尽最大可能过滤出去,把它们的影响减到最小。这就是卡尔曼滤波器要解决的问题。1.2标量卡尔曼滤波器卡尔曼对这个问题的解答就是卡尔曼滤波器。下面的流程图可以分成上下两个部分:上半部分就是问题本身,下半部分就是卡尔曼滤波器。第8页(共77页)卡尔曼滤波器简介(阎泓著u(-1)X()bh+(aX(t-1)bb(()2()+ak文-b)+Residual在图中,z1代表实际测量值,x代表过程的真值。此外在卡尔曼滤波器的流程图中出现了几种新的符号,分别是x代表先验估计( A priori estimate),和E代表后验估计(A posteriori estimate)4.对一个随机变量当前值的先验估计是根据前一个时刻以及更早的历史观测信息所作出的估计:后验估计是根据当前时刻以及更早的历史观测信息所作出的估计。x1的先验估计是由上一个时间点的后验估计值和输入信息给出的,x,=ax+ bur-p卡尔曼使用x的先验估计给出可测量E的(先验估计)预测5,而z,的实际测得值与预测值之间的差称为滤波过程的革新( nnovation)或者残余( Residua,即Residual=(10)本文采取通用的符号,以表示对某变量y在t时刻的后验估计,而表示对y的先验估计。在某些文献中y又记作y(|t-1),又记作y(t|t)5对于z,而言后验估计没有意义。z,是可观测量,在后验时刻已经有实际观测值了。第9页(共77页)卡尔曼滤波器简介(阎泓著残余反映了预测值和实际值之间的差别。残余为零的话,估计值和实际值完全吻合。如果残余很小,表明估计值很好,反之就不好。卡尔曼滤波器可以利用残余的这一信息改善对x,的估计,给出后验估计。也就是x=x:+k(Residual)=*+k(z,-hR-其中的k称作卡尔曼增益或卡尔曼混合系数( Blending factor)现在剩下的问题就是如何找到k的值,使得估计为最优。为此需要定义先验均方差和后验均方差。121最优的k值先验误差和后验误差分别定义为(12)它们的方差就是先验均方差和后验均方差P≡varP, =vale(13)最优的k值是使后验均方差为最小的值,就是下式成立时的k值(14)ak122计算先验均方差先验均方差为≡war(15)因为(2)式及(8)试式x,=ax_+ bu+we=ax+bu可得e:=x-x=ax+bu +w_)-(ax +bur=a(xx_1)+W因此第10页(共77页)