西乡建网站公司,网络推广培训一对一,免费虚拟云windows主机,浙江省建设厅 网站是多少目录 非线性系统泰勒展开卡尔曼滤波卡尔曼增益模型误差协方差矩阵 公式总结 本节默认你能够完整推导标准卡尔曼滤波#xff0c;将会简化一些推导的描述。如果你还不会完整推导标准卡尔曼滤波#xff0c;请先从
【卡尔曼滤波理论推导与实践】系列开始看起。 非线性系统
扩展… 目录 非线性系统泰勒展开卡尔曼滤波卡尔曼增益模型误差协方差矩阵 公式总结 本节默认你能够完整推导标准卡尔曼滤波将会简化一些推导的描述。如果你还不会完整推导标准卡尔曼滤波请先从
【卡尔曼滤波理论推导与实践】系列开始看起。 非线性系统
扩展卡尔曼滤波解决的是非线性系统其非线性状态空间方程为 { z k 模 ⃗ f ( z k − 1 模 ⃗ , F k − 1 ) y k 测 ⃗ h ( z k 测 ⃗ ) \begin{cases}\vec{z_{k模}}\mathrm{f}(\vec{z_{k-1模}},F_{k-1})\\\vec{y_{k测}}\mathrm{h}(\vec{z_{k测}})\end{cases} {zk模 f(zk−1模 ,Fk−1)yk测 h(zk测 ) f , h \mathrm{f},\mathrm{h} f,h函数是向量函数有多个输出 [ f 1 . . . f n ] \left[\begin{matrix}f_1\\...\\f_n\\\end{matrix}\right] f1...fn n n n是 z ⃗ \vec{z} z 的维数比如追踪平面图像物体坐标和速度 z ⃗ [ x y v x v y ] \vec{z}\left[\begin{matrix}x\\y\\v_x\\v_y\end{matrix}\right] z xyvxvy 。 其非线性真实系统模型为 { z k 真 ⃗ f ( z k − 1 真 ⃗ , F k − 1 , w k − 1 ⃗ ) y k 测 ⃗ h ( z k 真 ⃗ , v k ⃗ ) \begin{cases}\vec{z_{k真}}\mathrm{f}(\vec{z_{k-1真}},F_{k-1},\vec{w_{k-1}})\\\vec{y_{k测}}\mathrm{h}(\vec{z_{k真}},\vec{v_k})\end{cases} {zk真 f(zk−1真 ,Fk−1,wk−1 )yk测 h(zk真 ,vk )
因为非线性函数的期望不能用协方差矩阵描述无法用卡尔曼滤波的套路进行推导所以要进行线性化近似这就是扩展卡尔曼滤波与标准卡尔曼滤波的唯一区别。
泰勒展开
扩展卡尔曼滤波使用泰勒展开进行线性化近似扩展卡尔曼滤波是一个二元向量输入多输出系统比如输入为 [ z k − 1 模 ⃗ , w k − 1 ⃗ ] [\vec{z_{k-1模}},\vec{w_{k-1}}] [zk−1模 ,wk−1 ]这有两列向量输出为 z k 真 ⃗ \vec{z_{k真}} zk真 这个向量。 这里有两个公式要提前知道
单输出的二元函数在 ( x 0 , y 0 ) (x_0,y_0) (x0,y0)点的泰勒展开公式 f ( x , y ) f ( x 0 , x 0 ) d f d x ∣ x 0 , y 0 ∗ ( x − x 0 ) d f d y ∣ x 0 , y 0 ∗ ( y − y 0 ) f(x,y)f(x_0,x_0)\frac{df}{dx}|_{x_0,y_0}*(x-x_0)\frac{df}{dy}|_{x_0,y_0}*(y-y_0) f(x,y)f(x0,x0)dxdf∣x0,y0∗(x−x0)dydf∣x0,y0∗(y−y0)多输出函数对输入向量求导公式 d f ⃗ ( x ⃗ ) d x ⃗ [ d f 1 d x 1 d f 1 d x 2 ⋯ d f 1 d x m d f 2 d x 1 d f 2 d x 2 ⋯ d f 2 d x m ⋮ ⋮ ⋱ ⋮ d f n d x 1 d f n d x 2 ⋯ d f n d x m ] 雅可比矩阵 \frac{d\vec{f}(\vec{x})}{d\vec{x}}\left[\begin{matrix}\frac{d f_1}{d x_1} \frac{d f_1}{dx_2} \cdots \frac{df_1}{dx_m} \\ \frac{df_2}{dx_1} \frac{df_2}{dx_2} \cdots \frac{df_2}{dx_m} \\ \vdots \vdots \ddots \vdots \\ \frac{df_n}{dx_1} \frac{df_n}{dx_2} \cdots \frac{df_n}{dx_m}\end{matrix}\right]雅可比矩阵 dx df (x ) dx1df1dx1df2⋮dx1dfndx2df1dx2df2⋮dx2dfn⋯⋯⋱⋯dxmdf1dxmdf2⋮dxmdfn 雅可比矩阵
接下来进行泰勒展开 记 f \mathrm{f} f对 z ⃗ \vec{z} z 求导的雅可比矩阵为 A \mathrm{A} A f \mathrm{f} f对 w ⃗ \vec{w} w 求导的雅可比矩阵为 W \mathrm{W} W h \mathrm{h} h对 z ⃗ \vec{z} z 求导的雅可比矩阵为 H \mathrm{H} H h \mathrm{h} h对 v ⃗ \vec{v} v 求导的雅可比矩阵为 V \mathrm{V} V。 注意 F k − 1 F_{k-1} Fk−1是给定值而不是变量不参与泰勒展开噪声的期望平均值是零所以泰勒展开点的噪声值为零
可以在任意已知点比如模型值和上轮估计值都是已知的展开在哪个点展开都是为了方便推导。 状态转移方程分别在 z k − 1 估 ⃗ , z k 模 ⃗ \vec{z_{k-1估}},\vec{z_{k模}} zk−1估 ,zk模 处泰勒展开 { z k 模 ⃗ f ( z k − 1 估 ⃗ , F k − 1 ) A ∗ ( z k − 1 模 ⃗ − z k − 1 估 ⃗ ) y k 测 ⃗ h ( z k 模 ⃗ ) H ∗ ( z k 测 ⃗ − z k 模 ⃗ ) \begin{cases}\vec{z_{k模}}f(\vec{z_{k-1估}},F_{k-1})\mathrm{A}*(\vec{z_{k-1模}}-\vec{z_{k-1估}})\\\vec{y_{k测}}h(\vec{z_{k模}})\mathrm{H}*(\vec{z_{k测}}-\vec{z_{k模}})\end{cases} {zk模 f(zk−1估 ,Fk−1)A∗(zk−1模 −zk−1估 )yk测 h(zk模 )H∗(zk测 −zk模 ) 真实系统模型分别在 ( z k − 1 估 ⃗ , 0 ⃗ ) , ( z k 模 ⃗ , 0 ⃗ ) (\vec{z_{k-1估}},\vec{0}),(\vec{z_{k模}},\vec{0}) (zk−1估 ,0 ),(zk模 ,0 )泰勒展开 { z k 真 ⃗ f ( z k − 1 估 ⃗ , F k − 1 , 0 ⃗ ) A ∗ ( z k − 1 真 ⃗ − z k − 1 估 ⃗ ) W ∗ ( w k − 1 ⃗ − 0 ⃗ ) y k 测 ⃗ h ( z k 模 ⃗ , 0 ⃗ ) H ∗ ( z k 真 ⃗ − z k 模 ⃗ ) V ∗ ( v k ⃗ − 0 ⃗ ) \begin{cases}\vec{z_{k真}}f(\vec{z_{k-1估}},F_{k-1},\vec{0})\mathrm{A}*(\vec{z_{k-1真}}-\vec{z_{k-1估}})\mathrm{W}*(\vec{w_{k-1}}-\vec{0})\\\vec{y_{k测}}h(\vec{z_{k模}},\vec{0})\mathrm{H}*(\vec{z_{k真}}-\vec{z_{k模}})\mathrm{V}*(\vec{v_k}-\vec{0})\end{cases} {zk真 f(zk−1估 ,Fk−1,0 )A∗(zk−1真 −zk−1估 )W∗(wk−1 −0 )yk测 h(zk模 ,0 )H∗(zk真 −zk模 )V∗(vk −0 )
卡尔曼滤波
线性化后接下来的推导过程和标准卡尔曼滤波不能说一摸一样只能说九分甚至十分地相似。 这里再放一遍卡尔曼滤波核心思想 用测量值修正系统模型值得到当前轮次的最优估计值。再将该最优估计值当作下一轮的系统模型输入继续进行下一轮的最优估计如此迭代进行。 要最优化估计值首先把估计值表达式写出来 z k 估 ⃗ z k 模 ⃗ G k ∗ ( z k 测 ⃗ − z k 模 ⃗ ) \begin{aligned}\vec{z_{k估}}\vec{z_{k模}}\mathrm{G_k}*(\vec{z_{k测}}-\vec{z_{k模}})\end{aligned} zk估 zk模 Gk∗(zk测 −zk模 ) 对 G \mathrm{G} G矩阵进行一个变换变出一个 y k 测 ⃗ \vec{y_{k测}} yk测 出来方便推导令 G K ∗ H \mathrm{G}\mathrm{K}*\mathrm{H} GK∗H于是估计值表达式变为 z k 估 ⃗ z k 模 ⃗ K k ∗ H ∗ ( z k 测 ⃗ − z k 模 ⃗ ) z k 模 ⃗ K k ∗ ( y k 测 ⃗ − H ∗ z k 模 ⃗ ) \begin{aligned}\vec{z_{k估}}\vec{z_{k模}}\mathrm{K_k}*\mathrm{H}*(\vec{z_{k测}}-\vec{z_{k模}})\\\vec{z_{k模}}\mathrm{K_k}*(\vec{y_{k测}}-\mathrm{H}*\vec{z_{k模}})\end{aligned} zk估 zk模 Kk∗H∗(zk测 −zk模 )zk模 Kk∗(yk测 −H∗zk模 )
卡尔曼增益 z k 估 ⃗ \vec{z_{k估}} zk估 表达式右侧除了 K k \mathrm{K_k} Kk不知道其他都是已知数继续推导 K k \mathrm{K_k} Kk吧。那么就是推导最优估计下的 K k \mathrm{K_k} Kk就是使得估计误差协方差矩阵的迹最小先把估计误差协方差矩阵写出来 P k 估 E [ e k 估 ⃗ e k 估 ⃗ T ] E [ ( z k 真 ⃗ − z k 估 ⃗ ) ∗ ( z k 真 ⃗ − z k 估 ⃗ ) T ] \begin{aligned}\mathrm{P_{k估}}E[\vec{e_{k估}}\vec{e_{k估}}^T]\\E[(\vec{z_{k真}}-\vec{z_{k估}})*(\vec{z_{k真}}-\vec{z_{k估}})^T]\end{aligned} Pk估E[ek估 ek估 T]E[(zk真 −zk估 )∗(zk真 −zk估 )T] 展开一下 z k 真 ⃗ − z k 估 ⃗ \vec{z_{k真}}-\vec{z_{k估}} zk真 −zk估 : z k 真 ⃗ − z k 估 ⃗ z k 真 ⃗ − z k 模 ⃗ − K k ∗ ( y k 测 ⃗ − h ( z k 模 ⃗ ) ) e k 模 ⃗ − K k ∗ ( h ( z k 模 ⃗ , 0 ) H ∗ ( z k 真 ⃗ − z k 模 ⃗ ) V ∗ v k ⃗ − h ( z k 模 ⃗ ) ) e k 模 ⃗ − K k ∗ ( H ∗ e k 模 ⃗ V ∗ v k ⃗ ) ( I − K k H ) e k 模 ⃗ − K k V ∗ v k ⃗ \begin{aligned}\vec{z_{k真}}-\vec{z_{k估}}\vec{z_{k真}}-\vec{z_{k模}}-\mathrm{K_k}*(\vec{y_{k测}}-\mathrm{h}(\vec{z_{k模}}))\\\vec{e_{k模}}-\mathrm{K_k}*(h(\vec{z_{k模}},0)\mathrm{H}*(\vec{z_{k真}}-\vec{z_{k模}})\mathrm{V}*\vec{v_k}-h(\vec{z_{k模}}))\\\vec{e_{k模}}-\mathrm{K_k}*(\mathrm{H}*\vec{e_{k模}}\mathrm{V}*\vec{v_k})\\(\mathrm{I}-\mathrm{K_k}\mathrm{H})\vec{e_{k模}}-\mathrm{K_k}\mathrm{V}*\vec{v_k}\end{aligned} zk真 −zk估 zk真 −zk模 −Kk∗(yk测 −h(zk模 ))ek模 −Kk∗(h(zk模 ,0)H∗(zk真 −zk模 )V∗vk −h(zk模 ))ek模 −Kk∗(H∗ek模 V∗vk )(I−KkH)ek模 −KkV∗vk P k 估 \mathrm{P_{k估}} Pk估可以继续展开了 P k 估 E [ ( ( I − K k H ) e k 模 ⃗ − K k V ∗ v k ⃗ ) ∗ ( ( I − K k H ) e k 模 ⃗ − K k V ∗ v k ⃗ ) T ] E [ ( ( I − K k H ) e k 模 ⃗ − K k V ∗ v k ⃗ ) ∗ ( e k 模 ⃗ T ( I − K k H ) T − v k ⃗ T V T K k T ) ] \begin{aligned}\mathrm{P_{k估}}E[((\mathrm{I}-\mathrm{K_k}\mathrm{H})\vec{e_{k模}}-\mathrm{K_k}\mathrm{V}*\vec{v_k})*((\mathrm{I}-\mathrm{K_k}\mathrm{H})\vec{e_{k模}}-\mathrm{K_k}\mathrm{V}*\vec{v_k})^T]\\E[((\mathrm{I}-\mathrm{K_k}\mathrm{H})\vec{e_{k模}}-\mathrm{K_k}\mathrm{V}*\vec{v_k})*(\vec{e_{k模}}^T(\mathrm{I}-\mathrm{K_k}\mathrm{H})^T-\vec{v_k}^T\mathrm{V}^T\mathrm{K_k}^T)]\end{aligned} Pk估E[((I−KkH)ek模 −KkV∗vk )∗((I−KkH)ek模 −KkV∗vk )T]E[((I−KkH)ek模 −KkV∗vk )∗(ek模 T(I−KkH)T−vk TVTKkT)] 接下来将括号乘开。这里有两点有助于化简噪声 v k ⃗ \vec{v_k} vk 的期望是零又与 e k 模 ⃗ \vec{e_{k模}} ek模 相互独立所以 v k ⃗ \vec{v_k} vk 和 e k 模 ⃗ \vec{e_{k模}} ek模 搭边的项都是零。 P k 估 E [ ( I − K k H ) e k 模 ⃗ e k 模 ⃗ T ( I − K k H ) T K k V ∗ v k ⃗ v k ⃗ T V T K k T ] ( I − K k H ) P k 模 ( I − K k H ) T K k V R V T K k T \begin{aligned}\mathrm{P_{k估}}E[(\mathrm{I}-\mathrm{K_k}\mathrm{H})\vec{e_{k模}}\vec{e_{k模}}^T(\mathrm{I}-\mathrm{K_k}\mathrm{H})^T\mathrm{K_k}\mathrm{V}*\vec{v_k}\vec{v_k}^T\mathrm{V}^T\mathrm{K_k}^T]\\(\mathrm{I}-\mathrm{K_k}\mathrm{H})\mathrm{P_{k模}}(\mathrm{I}-\mathrm{K_k}\mathrm{H})^T\mathrm{K_k}\mathrm{V}\mathrm{R}\mathrm{V}^T\mathrm{K_k}^T\end{aligned} Pk估E[(I−KkH)ek模 ek模 T(I−KkH)TKkV∗vk vk TVTKkT](I−KkH)Pk模(I−KkH)TKkVRVTKkT P k 估 \mathrm{P_{k估}} Pk估的表达式也出来了 K k \mathrm{K_k} Kk是自变量想让 t r ( P k 估 ) tr(\mathrm{P_{k估}}) tr(Pk估)最小即最优估计。由于 t r ( P k 估 ) tr(\mathrm{P_{k估}}) tr(Pk估)是二次函数开口向上极值点必定是极小值点于是接下来准备 t r ( P k 估 ) d K k 0 \frac{tr(\mathrm{P_{k估}})}{d\mathrm{K_k}}0 dKktr(Pk估)0。在算极值点前有个矩阵求导公式需要提前知道 d t r ( A B A T ) d A 2 A B \frac{dtr(ABA^T)}{d\mathrm{A}}2AB dAdtr(ABAT)2AB 这个求导公式结合链式求导法则就可以进行求极值点了注意 V R V T \mathrm{V}\mathrm{R}\mathrm{V}^T VRVT看作一个整体 d P k 估 d K k 2 ( I − K k H ) P k 模 ( − H T ) 2 K k V R V T 0 \begin{aligned}\frac{d\mathrm{P_{k估}}}{d\mathrm{K_k}}2(\mathrm{I}-\mathrm{K_k}\mathrm{H})\mathrm{P_{k模}}(-\mathrm{H}^T)2\mathrm{K_k}\mathrm{V}\mathrm{R}\mathrm{V}^T\\0\end{aligned} dKkdPk估2(I−KkH)Pk模(−HT)2KkVRVT0 K k \mathrm{K_k} Kk这不就出来了和标准卡尔曼滤波里的 K k \mathrm{K_k} Kk挺像的 K k P k 模 H T H P k 模 H T V R V T \mathrm{K_k}\frac{\mathrm{P_{k模}}\mathrm{H}^T}{\mathrm{H}\mathrm{P_{k模}}\mathrm{H}^T\mathrm{V}\mathrm{R}\mathrm{V}^T} KkHPk模HTVRVTPk模HT
模型误差协方差矩阵
和标准卡尔曼滤波的推导套路一样继续推导 P k 模 \mathrm{P_{k模}} Pk模。 先把表达式搞出来 P k 模 E [ e k 模 ⃗ e k 模 ⃗ T ] E [ ( z k 真 ⃗ − z k 模 ⃗ ) ∗ ( z k 真 ⃗ − z k 模 ⃗ ) T ] \begin{aligned}\mathrm{P_{k模}}E[\vec{e_{k模}}\vec{e_{k模}}^T]\\E[(\vec{z_{k真}}-\vec{z_{k模}})*(\vec{z_{k真}}-\vec{z_{k模}})^T]\end{aligned} Pk模E[ek模 ek模 T]E[(zk真 −zk模 )∗(zk真 −zk模 )T] 展开一下 z k 真 ⃗ − z k 模 ⃗ \vec{z_{k真}}-\vec{z_{k模}} zk真 −zk模 z k 真 ⃗ − z k 模 ⃗ f ( z k − 1 估 ⃗ , F k − 1 , 0 ) A ∗ ( z k − 1 真 ⃗ − z k − 1 估 ⃗ ) W ∗ w k − 1 ⃗ − ( f ( z k − 1 估 ⃗ , F k − 1 ) A ∗ ( z k − 1 模 ⃗ − z k − 1 估 ⃗ ) ) A ( z k − 1 真 ⃗ − z k − 1 估 ⃗ ) W ∗ w k − 1 ⃗ A e k 估 ⃗ W w k − 1 ⃗ \begin{aligned}\vec{z_{k真}}-\vec{z_{k模}}f(\vec{z_{k-1估}},F_{k-1},0)\mathrm{A}*(\vec{z_{k-1真}}-\vec{z_{k-1估}})\mathrm{W}*\vec{w_{k-1}}-(f(\vec{z_{k-1估}},F_{k-1})\mathrm{A}*(\vec{z_{k-1模}}-\vec{z_{k-1估}}))\\\mathrm{A}(\vec{z_{k-1真}}-\vec{z_{k-1估}})\mathrm{W}*\vec{w_{k-1}}\\\mathrm{A}\vec{e_{k估}}\mathrm{W}\vec{w_{k-1}}\end{aligned} zk真 −zk模 f(zk−1估 ,Fk−1,0)A∗(zk−1真 −zk−1估 )W∗wk−1 −(f(zk−1估 ,Fk−1)A∗(zk−1模 −zk−1估 ))A(zk−1真 −zk−1估 )W∗wk−1 Aek估 Wwk−1 P k 模 \mathrm{P_{k模}} Pk模继续展开展开后的表达式和标准卡尔曼滤波里的 P k 模 \mathrm{P_{k模}} Pk模也挺像的 P k 模 E [ ( A e k − 1 估 ⃗ W w k − 1 ⃗ ) ∗ ( A e k − 1 估 ⃗ W w k − 1 ⃗ ) T ] E [ ( A e k − 1 估 ⃗ W w k − 1 ⃗ ) ∗ ( e k − 1 估 ⃗ T A T w k − 1 ⃗ T W T ) ] E [ A e k − 1 估 ⃗ e k − 1 估 ⃗ T A T W w k − 1 ⃗ w k − 1 ⃗ T W T ] A E [ e k − 1 估 ⃗ e k − 1 估 ⃗ T ] A T W E [ w k − 1 ⃗ w k − 1 ⃗ T ] W T A P k − 1 估 A T W Q W T \begin{aligned}\mathrm{P_{k模}}E[(\mathrm{A}\vec{e_{k-1估}}\mathrm{W}\vec{w_{k-1}})*(\mathrm{A}\vec{e_{k-1估}}\mathrm{W}\vec{w_{k-1}})^T]\\E[(\mathrm{A}\vec{e_{k-1估}}\mathrm{W}\vec{w_{k-1}})*(\vec{e_{k-1估}}^T\mathrm{A}^T\vec{w_{k-1}}^T\mathrm{W}^T)]\\E[\mathrm{A}\vec{e_{k-1估}}\vec{e_{k-1估}}^T\mathrm{A}^T\mathrm{W}\vec{w_{k-1}}\vec{w_{k-1}}^T\mathrm{W}^T]\\\mathrm{A}E[\vec{e_{k-1估}}\vec{e_{k-1估}}^T]\mathrm{A}^T\mathrm{W}E[\vec{w_{k-1}}\vec{w_{k-1}}^T]\mathrm{W}^T\\\mathrm{A}\mathrm{P_{k-1估}}\mathrm{A}^T\mathrm{W}\mathrm{Q}\mathrm{W}^T\end{aligned} Pk模E[(Aek−1估 Wwk−1 )∗(Aek−1估 Wwk−1 )T]E[(Aek−1估 Wwk−1 )∗(ek−1估 TATwk−1 TWT)]E[Aek−1估 ek−1估 TATWwk−1 wk−1 TWT]AE[ek−1估 ek−1估 T]ATWE[wk−1 wk−1 T]WTAPk−1估ATWQWT P k − 1 估 \mathrm{P_{k-1估}} Pk−1估表达式在上面推导过了只不过 k k k换成 k − 1 k-1 k−1 表达式 P k − 1 估 ( I − K k − 1 H ) P k − 1 模 ( I − H T K k − 1 T ) K k − 1 V R V T K k − 1 T K k − 1 P k − 1 模 H T H P k − 1 模 H T V R V T 表达式\mathrm{P_{k-1估}}(\mathrm{I}-\mathrm{K_{k-1}}\mathrm{H})\mathrm{P_{k-1模}}(\mathrm{I}-\mathrm{H}^T\mathrm{K_{k-1}}^T)\mathrm{K_{k-1}}\mathrm{V}\mathrm{R}\mathrm{V}^T\mathrm{K_{k-1}}^T\mathrm{K_{k-1}}\frac{\mathrm{P_{k-1模}}\mathrm{H}^T}{\mathrm{H}\mathrm{P_{k-1模}}\mathrm{H}^T\mathrm{V}\mathrm{R}\mathrm{V}^T} 表达式Pk−1估(I−Kk−1H)Pk−1模(I−HTKk−1T)Kk−1VRVTKk−1TKk−1HPk−1模HTVRVTPk−1模HT 把 K k − 1 \mathrm{K_{k-1}} Kk−1代到 P k − 1 估 \mathrm{P_{k-1估}} Pk−1估表达式里就得到 P k − 1 估 \mathrm{P_{k-1估}} Pk−1估的具体值你完全可以将 K k − 1 \mathrm{K_{k-1}} Kk−1直接无脑代入 P k − 1 估 \mathrm{P_{k-1估}} Pk−1估的表达式毕竟 P k − 1 估 \mathrm{P_{k-1估}} Pk−1估等式右边全是上轮迭代得到的已知数但是 P k − 1 估 \mathrm{P_{k-1估}} Pk−1估可以进行化简减少计算量 P k − 1 估 ( P k − 1 模 − K k − 1 H P k − 1 模 − P k − 1 模 H T K k − 1 T K k − 1 H P k − 1 模 H T K k − 1 T ) K k − 1 V R V T K k − 1 T ( P k − 1 模 − K k − 1 H P k − 1 模 − P k − 1 模 H T K k − 1 T ) K k − 1 ( H P k − 1 模 H T V R V T ) K k − 1 T ( P k − 1 模 − K k − 1 H P k − 1 模 − P k − 1 模 H T K k − 1 T ) P k − 1 模 H T K k − 1 T P k − 1 模 − K k − 1 H P k − 1 模 ( I − K k − 1 H ) P k − 1 模 \begin{aligned}\mathrm{P_{k-1估}}(\mathrm{P_{k-1模}}-\mathrm{K_{k-1}}\mathrm{H}\mathrm{P_{k-1模}}-\mathrm{P_{k-1模}}\mathrm{H}^T\mathrm{K_{k-1}}^T\mathrm{K_{k-1}}\mathrm{H}\mathrm{P_{k-1模}}\mathrm{H}^T\mathrm{K_{k-1}}^T)\mathrm{K_{k-1}}\mathrm{V}\mathrm{R}\mathrm{V}^T\mathrm{K_{k-1}}^T\\(\mathrm{P_{k-1模}}-\mathrm{K_{k-1}}\mathrm{H}\mathrm{P_{k-1模}}-\mathrm{P_{k-1模}}\mathrm{H}^T\mathrm{K_{k-1}}^T)\mathrm{K_{k-1}}(\mathrm{H}\mathrm{P_{k-1模}}\mathrm{H}^T\mathrm{V}\mathrm{R}\mathrm{V}^T)\mathrm{K_{k-1}}^T\\(\mathrm{P_{k-1模}}-\mathrm{K_{k-1}}\mathrm{H}\mathrm{P_{k-1模}}-\mathrm{P_{k-1模}}\mathrm{H}^T\mathrm{K_{k-1}}^T)\mathrm{P_{k-1模}}\mathrm{H}^T\mathrm{K_{k-1}}^T\\\mathrm{P_{k-1模}}-\mathrm{K_{k-1}}\mathrm{H}\mathrm{P_{k-1模}}\\(\mathrm{I}-\mathrm{K_{k-1}}\mathrm{H})\mathrm{P_{k-1模}}\end{aligned} Pk−1估(Pk−1模−Kk−1HPk−1模−Pk−1模HTKk−1TKk−1HPk−1模HTKk−1T)Kk−1VRVTKk−1T(Pk−1模−Kk−1HPk−1模−Pk−1模HTKk−1T)Kk−1(HPk−1模HTVRVT)Kk−1T(Pk−1模−Kk−1HPk−1模−Pk−1模HTKk−1T)Pk−1模HTKk−1TPk−1模−Kk−1HPk−1模(I−Kk−1H)Pk−1模 又是和标准卡尔曼滤波里的 P k − 1 估 \mathrm{P_{k-1估}} Pk−1估长得挺像的。 这样扩展卡尔曼滤波的公式全推导完了。
公式总结 z k 模 ⃗ f ( z k − 1 模 ⃗ , F k − 1 ) \vec{z_{k模}}\mathrm{f}(\vec{z_{k-1模}},F_{k-1}) zk模 f(zk−1模 ,Fk−1) P k 模 A P k − 1 估 A T W Q W T \mathrm{P_{k模}}\mathrm{A}\mathrm{P_{k-1估}}\mathrm{A}^T\mathrm{W}\mathrm{Q}\mathrm{W}^T Pk模APk−1估ATWQWT K k P k 模 H T H P k 模 H T V R V T \mathrm{K_k}\frac{\mathrm{P_{k模}}\mathrm{H}^T}{\mathrm{H}\mathrm{P_{k模}}\mathrm{H}^T\mathrm{V}\mathrm{R}\mathrm{V}^T} KkHPk模HTVRVTPk模HT z k 估 ⃗ z k 模 ⃗ K k ∗ ( y k 测 ⃗ − h ( z k 模 ⃗ ) ) \vec{z_{k估}}\vec{z_{k模}}\mathrm{K_k}*(\vec{y_{k测}}-\mathrm{h}(\vec{z_{k模}})) zk估 zk模 Kk∗(yk测 −h(zk模 )) P k 估 ( I − K k H ) P k 模 \mathrm{P_{k估}}(\mathrm{I}-\mathrm{K_k}\mathrm{H})\mathrm{P_{k模}} Pk估(I−KkH)Pk模
大致迭代过程和标准卡尔曼滤波一样这五个公式按照顺序遍历计算。要注意的是标准卡尔曼滤波的 A , V , W \mathrm{A},\mathrm{V},\mathrm{W} A,V,W都是固定的矩阵而扩展卡尔曼滤波中的 A , V , W \mathrm{A},\mathrm{V},\mathrm{W} A,V,W是雅可比矩阵每次迭代过程中都需要更新计算一次。
