实例介绍
【实例简介】
提供了kf，ekf，ukf的详细推导过程，从标量推导开始，进而转入矢量推导，非常详细
卡尔曼滤波器简介(阎泓著 第一步、时间更新 29 第二步、测量更新 “““““““+““44““““4 29 24特殊情况. 30 第一种情况、先验误差极小...-.----130 第二种情况、先验误差极大. 30 第三种情况、测量噪声极大.… 31 第三章、标量EKF 画,通通画 4“““““+44=“++“““++4“4“+“4“““-“++ 32 3.1非线性状态模型 .32 3.2模型线性化 3 3.2.1过程噪声项的线性化. 33 3.2.2测量噪声项的线性化...11-34 3.2.3过程和测量噪声项同时线性化 …35 324过程的线性化…035 3.25测量的线性化 …36 3.3EKF滤波器… 137 3.31应用卡尔曼滤波器. 37 33,2计算先验均方差 37 3.33计算后验均方差 37 3.3.4计算k值 4a“44444“;4444454a44“44444=424444441“如44444;44444“44.45“#4444444a44444444 38 33.5k值为最优时的后验均方差 38 34算法 39 第一步、时间更新 ………9 第二步、测量更新 39 3.5EKF的缺陷 44“==++++4=++44日+“44=“““+4 40 第四章、矢量EKF 41 41非线性矢量状态模型 41 42矢量模型线性化 单“““·***“““***“““““***“““***4““-***4““*“→“““*→*-““““““*“““*+4““→*“·““·““““* 42 42.1矢量泛函的泰勒展开 42.2过程噪声项的线性化 42 4.2.3测量噪声项的线性化. →“““#+4+“44“““-4+44→“““4“4+-““+ 43 424过程和测量噪声项同时线性化 44 42.5过程的线性化 4“““4““*“4““*→““*+“4“““““““*4“““4“““++4““44“““4“44““““七 426测量的线性化 “““““·+““““*““““+“““““““+4“““““““+4“““→·“““+“ 45 43矢量EKF滤波器 面面面面 46 画面和面面, 43.1应用矢量卡尔曼滤波器 44““++“44“““*44“““++444““4+444“+“44““““+44 46 43.2计算先验均方差 46 43.3计算后验均方差 4““+44““““44““““+→4““““+4““““4“44““““ .47 434计算k值 47 435k值为最优时的后验均方差 48 45算法 “““+““““*“““““+…““““*“+44““ 48 第一步、时间更新.… 49 第3页(共77页) 卡尔曼滤波器简介(阎泓著 第二步、测量更新 ““4--““44-4 49 4.4特殊情况. ““““4444“ 画画新通画通 49 第一种情况、先验误差极小. 画画,画画画园画画,画画画面请通 .50 第二种情况、先验误差极大….----50 第三种情况、测量噪声极大 44“““+44““=++“44“““+444““4+“44““44+ 50 第五章、标量无迹变换UT 52 51无迹变换的任务 52 52真值 “““““++“++4“4“““+4“++4“““““+““+“““““ 52 5.3无迹测试点11014 53.1标量的无迹测试点 ………1 54 532无迹权重系数 翻国口道 55 533统计性质公式 …55 54测试点的无迹变换.56 5.4.1从测试点得到后验期待值. 画画通通画画山通画画新 56 542从测试点得到后验方差 “““+4“++“4“++““平““上“““4““平中“+““““平“4+“= 57 5.5讨论 品aB444a日日+44日4日日“4日a4日+a日本“日日日和本上日和4日““4 58 第六章 矢量无迹变换UT 4“““4“44“““4++44“““4+““4+2+“++“4“++4=“++“““2++““““++““4+““““++ 59 61矢量微分回顾 59 61.1计算真值会用到的恒等式19 62矢量无迹变换的任务 中本““丰二“中““ 60 63真值 61 63无迹测试点 63.1矢量的无迹测试点 画面通自品面画画面自自通国画日画面国通画日通山国国画山山面通画山山丽右日日画画画画画山 63 632无迹权重系数64 633UT变换下的对称性 64测试点的无迹变换 65 64.1几个恒等式…65 642从测试点得到后验期待值.…---1----66 642从测试点得到后验协方差 .67 65讨论 68 第七章、无迹滤波器UKF 111169 71高维非线性问题.069 711标量特例 画画画画画画新画画画画画画 ““*#“““““44“…4“““““4““+““→““““44““4 70 72无迹滤波器 面,面面面面面面面“面画 70 721无迹测试点 ““*4“““““44““+44““““*44“““++444“““4““+“44“““““ 722无迹权重系数 通画画通画画通通画画通山请画画画画画画出画请画画副。 723先验估计 画画·画‘画 4““+44““““44““““+→4““““+““““+“444““““+4 72 724应用卡尔曼滤波器 73 7.2.5计算后验均方差… 73 7.2.6计算k值… 444““+44“““*44 74 73算法 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() Ib a x(t-1) 直线表示信号的传送,箭头代表传送的方向。流程图中的图标有三种,第一种方框图标代 表时间延迟,见下图 x(t) T X(t-1) 第二种方框图标代表乘法(增益),见下图 第6页(共77页) 卡尔曼滤波器简介(阎泓著 a ax 第三种圆形图标代表加法(混合),见下图 a-b+C b G 这些图标可以按照有意义的方式组合起来,描述一个差分方程。 必须指出,这些图标并不局限于标量情形,而且适用于矢量情形,譬如x为一个矢量,而 a和b可以为矩阵。 112加入白噪声 假设在这个线性过程中有一个噪声项v鬟 x2=ax21+bu-1+W1-1 则此方程式可以用下面的信号流程图表示 w(t=1) u(-1) 中+ b a X(-1) 假定这个噪声ν是一个高斯白噪声,它满足3 N(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( a x(t-1) 测量过程本身带有一个噪声ν,影响了测量的准确度。同样我们假定ν是一个白噪声 (,R)(R≥0) (")≥=0(s≠) 此外假定ν与w和u都没有关联,也即 ()=v)=0(s1) 114卡尔曼问题陈述 现在要考虑的是如何从可观测量z;的观测数据中得出x的最优估计值,把噪声w和v尽 最大可能过滤出去,把它们的影响减到最小。 这就是卡尔曼滤波器要解决的问题。 1.2标量卡尔曼滤波器 卡尔曼对这个问题的解答就是卡尔曼滤波器。下面的流程图可以分成上下两个部分:上半 部分就是问题本身,下半部分就是卡尔曼滤波器。 第8页(共77页) 卡尔曼滤波器简介(阎泓著 u(-1) X() b h+( a X(t-1) bb( () 2() + a k 文-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 ≡var P, =vale (13) 最优的k值是使后验均方差为最小的值,就是下式成立时的k值 (14) ak 122计算先验均方差 先验均方差为 ≡war (15) 因为(2)式及(8)试式 x,=ax_+ bu+we =ax+bu 可得 e:=x-x=ax+bu +w_)-(ax +bur=a(x x_1)+W 因此 第10页(共77页) 【实例截图】
【核心代码】
提供了kf，ekf，ukf的详细推导过程，从标量推导开始，进而转入矢量推导，非常详细
卡尔曼滤波器简介(阎泓著 第一步、时间更新 29 第二步、测量更新 “““““““+““44““““4 29 24特殊情况. 30 第一种情况、先验误差极小...-.----130 第二种情况、先验误差极大. 30 第三种情况、测量噪声极大.… 31 第三章、标量EKF 画,通通画 4“““““+44=“++“““++4“4“+“4“““-“++ 32 3.1非线性状态模型 .32 3.2模型线性化 3 3.2.1过程噪声项的线性化. 33 3.2.2测量噪声项的线性化...11-34 3.2.3过程和测量噪声项同时线性化 …35 324过程的线性化…035 3.25测量的线性化 …36 3.3EKF滤波器… 137 3.31应用卡尔曼滤波器. 37 33,2计算先验均方差 37 3.33计算后验均方差 37 3.3.4计算k值 4a“44444“;4444454a44“44444=424444441“如44444;44444“44.45“#4444444a44444444 38 33.5k值为最优时的后验均方差 38 34算法 39 第一步、时间更新 ………9 第二步、测量更新 39 3.5EKF的缺陷 44“==++++4=++44日+“44=“““+4 40 第四章、矢量EKF 41 41非线性矢量状态模型 41 42矢量模型线性化 单“““·***“““***“““““***“““***4““-***4““*“→“““*→*-““““““*“““*+4““→*“·““·““““* 42 42.1矢量泛函的泰勒展开 42.2过程噪声项的线性化 42 4.2.3测量噪声项的线性化. →“““#+4+“44“““-4+44→“““4“4+-““+ 43 424过程和测量噪声项同时线性化 44 42.5过程的线性化 4“““4““*“4““*→““*+“4“““““““*4“““4“““++4““44“““4“44““““七 426测量的线性化 “““““·+““““*““““+“““““““+4“““““““+4“““→·“““+“ 45 43矢量EKF滤波器 面面面面 46 画面和面面, 43.1应用矢量卡尔曼滤波器 44““++“44“““*44“““++444““4+444“+“44““““+44 46 43.2计算先验均方差 46 43.3计算后验均方差 4““+44““““44““““+→4““““+4““““4“44““““ .47 434计算k值 47 435k值为最优时的后验均方差 48 45算法 “““+““““*“““““+…““““*“+44““ 48 第一步、时间更新.… 49 第3页(共77页) 卡尔曼滤波器简介(阎泓著 第二步、测量更新 ““4--““44-4 49 4.4特殊情况. ““““4444“ 画画新通画通 49 第一种情况、先验误差极小. 画画,画画画园画画,画画画面请通 .50 第二种情况、先验误差极大….----50 第三种情况、测量噪声极大 44“““+44““=++“44“““+444““4+“44““44+ 50 第五章、标量无迹变换UT 52 51无迹变换的任务 52 52真值 “““““++“++4“4“““+4“++4“““““+““+“““““ 52 5.3无迹测试点11014 53.1标量的无迹测试点 ………1 54 532无迹权重系数 翻国口道 55 533统计性质公式 …55 54测试点的无迹变换.56 5.4.1从测试点得到后验期待值. 画画通通画画山通画画新 56 542从测试点得到后验方差 “““+4“++“4“++““平““上“““4““平中“+““““平“4+“= 57 5.5讨论 品aB444a日日+44日4日日“4日a4日+a日本“日日日和本上日和4日““4 58 第六章 矢量无迹变换UT 4“““4“44“““4++44“““4+““4+2+“++“4“++4=“++“““2++““““++““4+““““++ 59 61矢量微分回顾 59 61.1计算真值会用到的恒等式19 62矢量无迹变换的任务 中本““丰二“中““ 60 63真值 61 63无迹测试点 63.1矢量的无迹测试点 画面通自品面画画面自自通国画日画面国通画日通山国国画山山面通画山山丽右日日画画画画画山 63 632无迹权重系数64 633UT变换下的对称性 64测试点的无迹变换 65 64.1几个恒等式…65 642从测试点得到后验期待值.…---1----66 642从测试点得到后验协方差 .67 65讨论 68 第七章、无迹滤波器UKF 111169 71高维非线性问题.069 711标量特例 画画画画画画新画画画画画画 ““*#“““““44“…4“““““4““+““→““““44““4 70 72无迹滤波器 面,面面面面面面面“面画 70 721无迹测试点 ““*4“““““44““+44““““*44“““++444“““4““+“44“““““ 722无迹权重系数 通画画通画画通通画画通山请画画画画画画出画请画画副。 723先验估计 画画·画‘画 4““+44““““44““““+→4““““+““““+“444““““+4 72 724应用卡尔曼滤波器 73 7.2.5计算后验均方差… 73 7.2.6计算k值… 444““+44“““*44 74 73算法 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() Ib a x(t-1) 直线表示信号的传送,箭头代表传送的方向。流程图中的图标有三种,第一种方框图标代 表时间延迟,见下图 x(t) T X(t-1) 第二种方框图标代表乘法(增益),见下图 第6页(共77页) 卡尔曼滤波器简介(阎泓著 a ax 第三种圆形图标代表加法(混合),见下图 a-b+C b G 这些图标可以按照有意义的方式组合起来,描述一个差分方程。 必须指出,这些图标并不局限于标量情形,而且适用于矢量情形,譬如x为一个矢量,而 a和b可以为矩阵。 112加入白噪声 假设在这个线性过程中有一个噪声项v鬟 x2=ax21+bu-1+W1-1 则此方程式可以用下面的信号流程图表示 w(t=1) u(-1) 中+ b a X(-1) 假定这个噪声ν是一个高斯白噪声,它满足3 N(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( a x(t-1) 测量过程本身带有一个噪声ν,影响了测量的准确度。同样我们假定ν是一个白噪声 (,R)(R≥0) (")≥=0(s≠) 此外假定ν与w和u都没有关联,也即 ()=v)=0(s1) 114卡尔曼问题陈述 现在要考虑的是如何从可观测量z;的观测数据中得出x的最优估计值,把噪声w和v尽 最大可能过滤出去,把它们的影响减到最小。 这就是卡尔曼滤波器要解决的问题。 1.2标量卡尔曼滤波器 卡尔曼对这个问题的解答就是卡尔曼滤波器。下面的流程图可以分成上下两个部分:上半 部分就是问题本身,下半部分就是卡尔曼滤波器。 第8页(共77页) 卡尔曼滤波器简介(阎泓著 u(-1) X() b h+( a X(t-1) bb( () 2() + a k 文-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 ≡var P, =vale (13) 最优的k值是使后验均方差为最小的值,就是下式成立时的k值 (14) ak 122计算先验均方差 先验均方差为 ≡war (15) 因为(2)式及(8)试式 x,=ax_+ bu+we =ax+bu 可得 e:=x-x=ax+bu +w_)-(ax +bur=a(x x_1)+W 因此 第10页(共77页) 【实例截图】
【核心代码】
标签：
好例子网口号：伸出你的我的手 — 分享！
小贴士
感谢您为本站写下的评论，您的评论对其它用户来说具有重要的参考价值，所以请认真填写。
- 类似“顶”、“沙发”之类没有营养的文字，对勤劳贡献的楼主来说是令人沮丧的反馈信息。
- 相信您也不想看到一排文字/表情墙，所以请不要反馈意义不大的重复字符，也请尽量不要纯表情的回复。
- 提问之前请再仔细看一遍楼主的说明，或许是您遗漏了。
- 请勿到处挖坑绊人、招贴广告。既占空间让人厌烦，又没人会搭理，于人于己都无利。
关于好例子网
本站旨在为广大IT学习爱好者提供一个非营利性互相学习交流分享平台。本站所有资源都可以被免费获取学习研究。本站资源来自网友分享，对搜索内容的合法性不具有预见性、识别性、控制性，仅供学习研究，请务必在下载后24小时内给予删除，不得用于其他任何用途，否则后果自负。基于互联网的特殊性，平台无法对用户传输的作品、信息、内容的权属或合法性、安全性、合规性、真实性、科学性、完整权、有效性等进行实质审查；无论平台是否已进行审查，用户均应自行承担因其传输的作品、信息、内容而可能或已经产生的侵权或权属纠纷等法律责任。本站所有资源不代表本站的观点或立场，基于网友分享，根据中国法律《信息网络传播权保护条例》第二十二与二十三条之规定，若资源存在侵权或相关问题请联系本站客服人员，点此联系我们。关于更多版权及免责申明参见 版权及免责申明
网友评论
我要评论