滤波算法_扩展卡尔曼滤波(EKF, Extended Kalman filter)_全网最详细的数学推导_Part1
本文提供全网最详细的扩展卡尔曼滤波的全部数学推导。由于推导公式很多，CSDN嫌我的文章太长了，所以我分成了Part1和Part2。
1、简单复习标准卡尔曼滤波的五大公式
对于线性系统，
{ x k = A x k − 1 + B u k + w k − 1 z k = H x k + v k \left\{ \begin{array}{l} {{\bf{x}}_k}{\bf{ = A}}{{\bf{x}}_{k - 1}}{\bf{ + B}}{{\bf{u}}_{\bf{k}}}{\bf{ + }}{{\bf{w}}_{k - 1}}\\\\ {{\bf{z}}_k}{\bf{ = H}}{{\bf{x}}_k}{\bf{ + }}{{\bf{v}}_k} \end{array} \right. ⎩ ⎨ ⎧xk=Axk−1+Buk+wk−1zk=Hxk+vk
其中， x k {{\bf{x}}_k} xk为状态向量， u k {{\bf{u}}_{\bf{k}}} uk为系统的输入向量， w k − 1 {{\bf{w}}_{k - 1}} wk−1为过程噪声， z k {{\bf{z}}_k} zk为观测向量， v k {{\bf{v}}_k} vk为观测噪声。其中， p ( w ) ∼ N ( 0 , Q ) p\left( {\bf{w}} \right) \sim N\left( {{\bf{0}},{\bf{Q}}} \right) p(w)∼N(0,Q), 其中 0 {\bf{0}} 0代表着期望， Q {\bf{Q}} Q代表的是过程噪声的协方差矩阵。 p ( v ) ∼ N ( 0 , R ) p\left( {\bf{v}} \right) \sim N\left( {{\bf{0}},{\bf{R}}} \right) p(v)∼N(0,R), R {\bf{R}} R代表的是测量噪声的协方差矩阵。
标准卡尔曼滤波有五大公式
模型估计（先验估计公式）：
x ^ k − = A x ^ k − 1 + B u k {\bf{\hat x}}_k^ - = {\bf{A}}{{\bf{\hat x}}_{k - 1}}{\bf{ + B}}{{\bf{u}}_{\bf{k}}} x^k−=Ax^k−1+Buk
模型估计与测量估计的数据融合公式（后验估计公式）：
x ^ k = x ^ k − + K k ( z k − H x ^ k − ) {\bf{\hat x}}_k^{} = {\bf{\hat x}}_k^ - + {{\bf{K}}_k}\left( {{{\bf{z}}_k} - {\bf{H\hat x}}_k^ - } \right) x^k=x^k−+Kk(zk−Hx^k−)
卡尔曼增益计算公式：
K k = P k − H T ( H P k − H T + R ) − 1 {{\bf{K}}_k} = {\bf{P}}_k^ - {{\bf{H}}^T}{\left( {{\bf{HP}}_k^ - {{\bf{H}}^T} + {\bf{R}}} \right)^{ - 1}} Kk=Pk−HT(HPk−HT+R)−1
先验估计噪声协方差更新公式：
P k − = A P k − 1 A T + Q {\bf{P}}_k^ - = {\bf{AP}}_{k - 1}^{}{{\bf{A}}^T} + {\bf{Q}} Pk−=APk−1AT+Q
后验估计噪声协方差更新公式：
P k = ( I − K k H ) P k − {\bf{P}}_k^{} = \left( {{\bf{I}} - {{\bf{K}}_k}{\bf{H}}} \right){\bf{P}}_k^ - Pk=(I−KkH)Pk−
在运行递归滤波算法时，需要我们给出0时刻的后验估计值初值 x ^ 0 {\bf{\hat x}}_0^{} x^0，以及后验估计误差协方差矩阵初值 P 0 {\bf{P}}_0^{} P0。
2、扩展卡尔曼滤波的数学建模
对于一个非线性系统
{ x k = f ( x k − 1 , w k − 1 , u k ) z k = h ( x k , v k ) \left\{ \begin{array}{l} {{\bf{x}}_k}{\bf{ = }}f\left( {{{\bf{x}}_{k - 1}},{{\bf{w}}_{k - 1}},{{\bf{u}}_k}} \right)\\ {{\bf{z}}_k}{\bf{ = }}h\left( {{{\bf{x}}_k},{{\bf{v}}_k}} \right) \end{array} \right. {xk=f(xk−1,wk−1,uk)zk=h(xk,vk)
注意： u k {{\bf{u}}_k} uk 是输入，是一个确定的量，它不属于随机变量。
数学基础补充
在对这个系统进行线性化之前，需要了解二元函数的泰勒展开公式
f ( x , y ) = f ( x k , y k ) + ∂ f ( x , y ) ∂ x ∣ x k , y k ⋅ ( x − x k ) + ∂ f ( x , y ) ∂ y ∣ x k , y k ⋅ ( y − y k ) + ⋯ + o n f\left( {x,y} \right) = f\left( {{x_k},{y_k}} \right) + \frac{{\partial f\left( {x,y} \right)}}{{\partial x}}{|_{{x_{k,}}{y_k}}} \cdot \left( {x - {x_k}} \right) + \frac{{\partial f\left( {x,y} \right)}}{{\partial y}}{|_{{x_{k,}}{y_k}}} \cdot \left( {y - {y_k}} \right) + \cdots + {o^n} f(x,y)=f(xk,yk)+∂x∂f(x,y)∣xk,yk⋅(x−xk)+∂y∂f(x,y)∣xk,yk⋅(y−yk)+⋯+on
下面对系统进行线性化，由于系统存在误差，无法在真实点 ( x k − 1 , w k − 1 ) \left( {{{\bf{x}}_{k - 1}},{{\bf{w}}_{k - 1}}} \right) (xk−1,wk−1)线性化，因此，我们在 ( x ^ k − 1 , 0 ) \left( {{{{\bf{\hat x}}}_{k - 1}},{\bf{0}}} \right) (x^k−1,0)处对 x k = f ( x k − 1 , w k − 1 , u k ) {{\bf{x}}_k}{\bf{ = }}f\left( {{{\bf{x}}_{k - 1}},{{\bf{w}}_{k - 1}},{{\bf{u}}_k}} \right) xk=f(xk−1,wk−1,uk)进行线性化，注意：不要把 u k {{\bf{u}}_k} uk 当成随机变量。
x k = f ( x ^ k − 1 , 0 , u k ) + ∂ f ∂ x k − 1 ∣ x k − 1 = x ^ k − 1 , w k − 1 = 0 , u k = u k ⋅ ( x k − 1 − x ^ k − 1 ) + ∂ f ∂ w k − 1 ∣ x k − 1 = x ^ k − 1 , w k − 1 = 0 , u k = u k ⋅ ( w k − 1 − 0 ) {{\bf{x}}_k}{\bf{ = }}f\left( {{{{\bf{\hat x}}}_{k - 1}},{\bf{0}},{{\bf{u}}_k}} \right) + \frac{{\partial f}}{{\partial {{\bf{x}}_{k - 1}}}}{|_{{{\bf{x}}_{k - 1}} = {{{\bf{\hat x}}}_{k - 1}},{{\bf{w}}_{k - 1}} = {\bf{0}},{{\bf{u}}_k} = {{\bf{u}}_k}}} \cdot \left( {{{\bf{x}}_{k - 1}} - {{{\bf{\hat x}}}_{k - 1}}} \right) + \frac{{\partial f}}{{\partial {{\bf{w}}_{k - 1}}}}{|_{{{\bf{x}}_{k - 1}} = {{{\bf{\hat x}}}_{k - 1}},{{\bf{w}}_{k - 1}} = {\bf{0}},{{\bf{u}}_k} = {{\bf{u}}_k}}} \cdot \left( {{{\bf{w}}_{k - 1}} - {\bf{0}}} \right) xk=f(x^k−1,0,uk)+∂xk−1∂f∣xk−1=x^k−1,wk−1=0,uk=uk⋅(xk−1−x^k−1)+∂wk−1∂f∣xk−1=x^k−1,wk−1=0,uk=uk⋅(wk−1−0)
我们定义，
x ~ k ≜ f ( x ^ k − 1 , 0 , u k ) A k ≜ ∂ f ∂ x k − 1 ∣ x k − 1 = x ^ k − 1 , w k − 1 = 0 , u k = u k W k ≜ ∂ f ∂ w k − 1 ∣ x k − 1 = x ^ k − 1 , w k − 1 = 0 , u k = u k \begin{array}{l} {{{\bf{\tilde x}}}_k} \triangleq f\left( {{{{\bf{\hat x}}}_{k - 1}},{\bf{0}},{{\bf{u}}_k}} \right)\\\\ {{\bf{A}}_k} \triangleq \frac{{\partial f}}{{\partial {{\bf{x}}_{k - 1}}}}{|_{{{\bf{x}}_{k - 1}} = {{{\bf{\hat x}}}_{k - 1}},{{\bf{w}}_{k - 1}} = {\bf{0}},{{\bf{u}}_k} = {{\bf{u}}_k}}}\\\\ {{\bf{W}}_k} \triangleq \frac{{\partial f}}{{\partial {{\bf{w}}_{k - 1}}}}{|_{{{\bf{x}}_{k - 1}} = {{{\bf{\hat x}}}_{k - 1}},{{\bf{w}}_{k - 1}} = {\bf{0}},{{\bf{u}}_k} = {{\bf{u}}_k}}} \end{array} x~k≜f(x^k−1,0,uk)Ak≜∂xk−1∂f∣xk−1=x^k−1,wk−1=0,uk=ukWk≜∂wk−1∂f∣xk−1=x^k−1,wk−1=0,uk=uk
A k {{\mathbf{A}}_k} Ak和 W k {{\mathbf{W}}_k} Wk被称为雅可比矩阵。
原式化简为：
x k = x ~ k + A k ( x k − 1 − x ^ k − 1 ) + W k w k − 1 {{\mathbf{x}}_k}{\mathbf{ = }}{{\mathbf{\tilde x}}_k} + {{\mathbf{A}}_k}\left( {{{\mathbf{x}}_{k - 1}} - {{{\mathbf{\hat x}}}_{k - 1}}} \right) + {{\mathbf{W}}_k}{{\mathbf{w}}_{k - 1}} xk=x~k+Ak(xk−1−x^k−1)+Wkwk−1
另外，我们定义 x k {{\mathbf{x}}_k} xk的先验估计 x ^ k − ≜ f ( x ^ k − 1 , 0 , u k ) = x ~ k {\mathbf{\hat x}}_k^ - \triangleq f\left( {{{{\mathbf{\hat x}}}_{k - 1}},{\mathbf{0}},{{\mathbf{u}}_k}} \right) = {{\mathbf{\tilde x}}_k} x^k−≜f(x^k−1,0,uk)=x~k
注意：非线性系统的状态转移方程在利用泰勒公式在 ( x ^ k − 1 , 0 ) \left( {{{{\mathbf{\hat x}}}_{k - 1}},{\mathbf{0}}} \right) (x^k−1,0)处进行线性化时，产生的第一项 x ~ k {{\mathbf{\tilde x}}_k} x~k它正好是 x k {{\mathbf{x}}_k} xk在k时刻的先验估计 x ^ k − {\mathbf{\hat x}}_k^ - x^k−。
因此原式亦可化简为
x k = x ^ k − + A k ( x k − 1 − x ^ k − 1 ) + W k w k − 1 {{\mathbf{x}}_k}{\mathbf{ = \hat x}}_k^ - + {{\mathbf{A}}_k}\left( {{{\mathbf{x}}_{k - 1}} - {{{\mathbf{\hat x}}}_{k - 1}}} \right) + {{\mathbf{W}}_k}{{\mathbf{w}}_{k - 1}} xk=x^k−+Ak(xk−1−x^k−1)+Wkwk−1
下面举个计算雅可比矩阵的例子，假设状态变量是2维的
x k = [ x k ( 1 ) x k ( 2 ) ] , w = [ w k ( 1 ) w k ( 2 ) ] , u = [ u k ( 1 ) u k ( 2 ) ] {{\bf{x}}_k} = \left[ {\begin{array}{l} {{x_k}\left( 1 \right)}\\ {{x_k}\left( 2 \right)} \end{array}} \right],{\bf{w}} = \left[ {\begin{array}{l} {{w_k}\left( 1 \right)}\\ {{w_k}\left( 2 \right)} \end{array}} \right],{\bf{u}} = \left[ {\begin{array}{l} {{u_k}\left( 1 \right)}\\ {{u_k}\left( 2 \right)} \end{array}} \right] xk=[xk(1)xk(2)],w=[wk(1)wk(2)],u=[uk(1)uk(2)]
f 1 = x k ( 1 ) = x k − 1 ( 1 ) + sin x k − 1 ( 2 ) + u k ( 1 ) + w k − 1 ( 1 ) + w k − 1 ( 1 ) 2 f 2 = x k ( 2 ) = x k − 1 ( 1 ) 2 + u k ( 2 ) + w k − 1 ( 1 ) + 2 w k − 1 ( 2 ) \begin{array}{l} {f_1} = {x_k}\left( 1 \right) = {x_{k - 1}}\left( 1 \right) + \sin {x_{k - 1}}\left( 2 \right) + {u_k}\left( 1 \right) + {w_{k - 1}}\left( 1 \right) + {w_{k - 1}}{\left( 1 \right)^2}\\ {f_2} = {x_k}\left( 2 \right) = {x_{k - 1}}{\left( 1 \right)^2} + {u_k}\left( 2 \right) + {w_{k - 1}}\left( 1 \right) + 2{w_{k - 1}}\left( 2 \right) \end{array} f1=xk(1)=xk−1(1)+sinxk−1(2)+uk(1)+wk−1(1)+wk−1(1)2f2=xk(2)=xk−1(1)2+uk(2)+wk−1(1)+2wk−1(2)
A k = [ ∂ f 1 ∂ x k − 1 ( 1 ) ∂ f 1 ∂ x k − 1 ( 2 ) ∂ f 2 ∂ x k − 1 ( 1 ) ∂ f 2 ∂ x k − 1 ( 2 ) ] ∣ x k − 1 = x ^ k − 1 , w k − 1 = 0 , u k = u k = [ 1 cos x k − 1 ( 2 ) 2 x k − 1 ( 1 ) 0 ] ∣ x k − 1 = x ^ k − 1 , w k − 1 = 0 , u k = u k = [ 1 cos x ^ k − 1 ( 2 ) 2 x ^ k − 1 ( 1 ) 0 ] \begin{aligned} {{\bf{A}}_k} &= \left[ {\begin{array}{l} {\frac{{\partial {f_1}}}{{\partial {x_{k - 1}}\left( 1 \right)}}}&{\frac{{\partial {f_1}}}{{\partial {x_{k - 1}}\left( 2 \right)}}}\\ {\frac{{\partial {f_2}}}{{\partial {x_{k - 1}}\left( 1 \right)}}}&{\frac{{\partial {f_2}}}{{\partial {x_{k - 1}}\left( 2 \right)}}} \end{array}} \right]{|_{{{\bf{x}}_{k - 1}} = {{{\bf{\hat x}}}_{k - 1}},{{\bf{w}}_{k - 1}} = {\bf{0}},{{\bf{u}}_k} = {{\bf{u}}_k}}}\\\\ &= \left[ {\begin{array}{l} 1&{\cos {x_{k - 1}}\left( 2 \right)}\\ {2{x_{k - 1}}\left( 1 \right)}&0 \end{array}} \right]{|_{{{\bf{x}}_{k - 1}} = {{{\bf{\hat x}}}_{k - 1}},{{\bf{w}}_{k - 1}} = {\bf{0}},{{\bf{u}}_k} = {{\bf{u}}_k}}} = \left[ {\begin{array}{l} 1&{\cos {{\hat x}_{k - 1}}\left( 2 \right)}\\ {2{{\hat x}_{k - 1}}\left( 1 \right)}&0 \end{array}} \right] \end{aligned} Ak=[∂xk−1(1)∂f1∂xk−1(1)∂f2∂xk−1(2)∂f1∂xk−1(2)∂f2]∣xk−1=x^k−1,wk−1=0,uk=uk=[12xk−1(1)cosxk−1(2)0]∣xk−1=x^k−1,wk−1=0,uk=uk=[12x^k−1(1)cosx^k−1(2)0]
W k = [ ∂ f 1 ∂ w k ( 1 ) ∂ f 1 ∂ w k ( 2 ) ∂ f 2 ∂ w k ( 1 ) ∂ f 2 ∂ w k ( 2 ) ] ∣ x k − 1 = x ^ k − 1 , w k − 1 = 0 , u k = u k = [ 1 + 2 w k − 1 ( 1 ) 0 1 2 ] ∣ x k − 1 = x ^ k − 1 , w k − 1 = 0 , u k = u k = [ 1 0 1 2 ] \begin{aligned} {{\bf{W}}_k} &= \left[ {\begin{array}{l} {\frac{{\partial {f_1}}}{{\partial {w_k}\left( 1 \right)}}}&{\frac{{\partial {f_1}}}{{\partial {w_k}\left( 2 \right)}}}\\ {\frac{{\partial {f_2}}}{{\partial {w_k}\left( 1 \right)}}}&{\frac{{\partial {f_2}}}{{\partial {w_k}\left( 2 \right)}}} \end{array}} \right]{|_{{{\bf{x}}_{k - 1}} = {{{\bf{\hat x}}}_{k - 1}},{{\bf{w}}_{k - 1}} = {\bf{0}},{{\bf{u}}_k} = {{\bf{u}}_k}}}\\\\ &= \left[ {\begin{array}{l} {1 + 2{w_{k - 1}}\left( 1 \right)}&0\\ 1&2 \end{array}} \right]{|_{{{\bf{x}}_{k - 1}} = {{{\bf{\hat x}}}_{k - 1}},{{\bf{w}}_{k - 1}} = {\bf{0}},{{\bf{u}}_k} = {{\bf{u}}_k}}} = \left[ {\begin{array}{l} 1&0\\ 1&2 \end{array}} \right] \end{aligned} Wk=[∂wk(1)∂f1∂wk(1)∂f2∂wk(2)∂f1∂wk(2)∂f2]∣xk−1=x^k−1,wk−1=0,uk=uk=[1+2wk−1(1)102]∣xk−1=x^k−1,wk−1=0,uk=uk=[1102]
下面，我们对 z k = h ( x k , v k ) {{\bf{z}}_k}{\bf{ = }}h\left( {{{\bf{x}}_k},{{\bf{v}}_k}} \right) zk=h(xk,vk)进行线性化， x k = f ( x k − 1 , w k − 1 , u k ) {{\bf{x}}_k}{\bf{ = }}f\left( {{{\bf{x}}_{k - 1}},{{\bf{w}}_{k - 1}},{{\bf{u}}_k}} \right) xk=f(xk−1,wk−1,uk) 的线性化是在 ( x ^ k − 1 , 0 ) \left( {{{{\bf{\hat x}}}_{k - 1}},{\bf{0}}} \right) (x^k−1,0)处，而 z k = h ( x k − 1 , v k ) {{\bf{z}}_k}{\bf{ = }}h\left( {{{\bf{x}}_{k - 1}},{{\bf{v}}_k}} \right) zk=h(xk−1,vk)我们选在 ( x ~ k , 0 ) \left( {{{{\bf{\tilde x}}}_k},{\bf{0}}} \right) (x~k,0) ，同样也是 ( x ^ k − , 0 ) \left( {{\bf{\hat x}}_k^ - ,{\bf{0}}} \right) (x^k−,0)。
即
z k = h ( x ^ k − , 0 ) + ∂ h ∂ x k ∣ x k = x ^ k − , v k = 0 ⋅ ( x k − x ^ k − ) + ∂ h ∂ v k ∣ x k = x ^ k − , v k = 0 ⋅ ( v k − 0 ) {{\bf{z}}_k} = h\left( {{\bf{\hat x}}_k^ - ,{\bf{0}}} \right) + \frac{{\partial h}}{{\partial {{\bf{x}}_k}}}{|_{{{\bf{x}}_k} = {\bf{\hat x}}_k^ - ,{{\bf{v}}_k} = {\bf{0}}}} \cdot \left( {{{\bf{x}}_k} - {\bf{\hat x}}_k^ - } \right) + \frac{{\partial h}}{{\partial {{\bf{v}}_k}}}{|_{{{\bf{x}}_k} = {\bf{\hat x}}_k^ - ,{{\bf{v}}_k} = {\bf{0}}}} \cdot \left( {{{\bf{v}}_k} - {\bf{0}}} \right) zk=h(x^k−,0)+∂xk∂h∣xk=x^k−,vk=0⋅(xk−x^k−)+∂vk∂h∣xk=x^k−,vk=0⋅(vk−0)
类似地，我们定义
z ^ k − ≜ h ( x ^ k − , 0 ) H k ≜ ∂ h ∂ x k ∣ x k = x ^ k − , v k = 0 V k ≜ ∂ f ∂ v k ∣ x k = x ^ k − , v k = 0 \begin{aligned} {\bf{\hat z}}_k^ - &\triangleq h\left( {{\bf{\hat x}}_k^ - ,{\bf{0}}} \right)\\\\ {{\bf{H}}_k} &\triangleq \frac{{\partial h}}{{\partial {{\bf{x}}_k}}}{|_{{{\bf{x}}_k} = {\bf{\hat x}}_k^ - ,{{\bf{v}}_k} = {\bf{0}}}}\\\\ {{\bf{V}}_k} &\triangleq \frac{{\partial f}}{{\partial {{\bf{v}}_k}}}{|_{{{\bf{x}}_k} = {\bf{\hat x}}_k^ - ,{{\bf{v}}_k} = {\bf{0}}}} \end{aligned} z^k−HkVk≜h(x^k−,0)≜∂xk∂h∣xk=x^k−,vk=0≜∂vk∂f∣xk=x^k−,vk=0
原式化简为：
z k = z ^ k − + H k ⋅ ( x k − x ^ k − ) + V k v k {{\mathbf{z}}_k} = {\mathbf{\hat z}}_k^ - + {{\mathbf{H}}_k} \cdot \left( {{{\mathbf{x}}_k} - {\mathbf{\hat x}}_k^ - } \right) + {{\mathbf{V}}_k}{{\mathbf{v}}_k} zk=z^k−+Hk⋅(xk−x^k−)+Vkvk
整理线性化的结果：
x k = x ^ k − + A k ( x k − 1 − x ^ k − 1 ) + W k w k − 1 {{\mathbf{x}}_k}{\mathbf{ = \hat x}}_k^ - + {{\mathbf{A}}_k}\left( {{{\mathbf{x}}_{k - 1}} - {{{\mathbf{\hat x}}}_{k - 1}}} \right) + {{\mathbf{W}}_k}{{\mathbf{w}}_{k - 1}} xk=x^k−+Ak(xk−1−x^k−1)+Wkwk−1
z k = z ^ k − + H k ⋅ ( x k − x ^ k − ) + V k v k {{\mathbf{z}}_k} = {\mathbf{\hat z}}_k^ - + {{\mathbf{H}}_k} \cdot \left( {{{\mathbf{x}}_k} - {\mathbf{\hat x}}_k^ - } \right) + {{\mathbf{V}}_k}{{\mathbf{v}}_k} zk=z^k−+Hk⋅(xk−x^k−)+Vkvk
x ^ k − ≜ x ~ k ≜ f ( x ^ k − 1 , 0 , u k ) A k ≜ ∂ f ∂ x k − 1 ∣ x k − 1 = x ^ k − 1 , w k − 1 = 0 , u k = u k W k ≜ ∂ f ∂ w k − 1 ∣ x k − 1 = x ^ k − 1 , w k − 1 = 0 , u k = u k \begin{array}{l} {\mathbf{\hat x}}_k^ -\triangleq{{{\bf{\tilde x}}}_k} \triangleq f\left( {{{{\bf{\hat x}}}_{k - 1}},{\bf{0}},{{\bf{u}}_k}} \right)\\\\ {{\bf{A}}_k} \triangleq \frac{{\partial f}}{{\partial {{\bf{x}}_{k - 1}}}}{|_{{{\bf{x}}_{k - 1}} = {{{\bf{\hat x}}}_{k - 1}},{{\bf{w}}_{k - 1}} = {\bf{0}},{{\bf{u}}_k} = {{\bf{u}}_k}}}\\\\ {{\bf{W}}_k} \triangleq \frac{{\partial f}}{{\partial {{\bf{w}}_{k - 1}}}}{|_{{{\bf{x}}_{k - 1}} = {{{\bf{\hat x}}}_{k - 1}},{{\bf{w}}_{k - 1}} = {\bf{0}},{{\bf{u}}_k} = {{\bf{u}}_k}}} \end{array} x^k−≜x~k≜f(x^k−1,0,uk)Ak≜∂xk−1∂f∣xk−1=x^k−1,wk−1=0,uk=ukWk≜∂wk−1∂f∣xk−1=x^k−1,wk−1=0,uk=uk
z ^ k − ≜ h ( x ^ k − , 0 ) H k ≜ ∂ h ∂ x k ∣ x k = x ^ k − , v k = 0 V k ≜ ∂ f ∂ v k ∣ x k = x ^ k − , v k = 0 \begin{aligned} {\bf{\hat z}}_k^ - &\triangleq h\left( {{\bf{\hat x}}_k^ - ,{\bf{0}}} \right)\\\\ {{\bf{H}}_k} &\triangleq \frac{{\partial h}}{{\partial {{\bf{x}}_k}}}{|_{{{\bf{x}}_k} = {\bf{\hat x}}_k^ - ,{{\bf{v}}_k} = {\bf{0}}}}\\\\ {{\bf{V}}_k} &\triangleq \frac{{\partial f}}{{\partial {{\bf{v}}_k}}}{|_{{{\bf{x}}_k} = {\bf{\hat x}}_k^ - ,{{\bf{v}}_k} = {\bf{0}}}} \end{aligned} z^k−HkVk≜h(x^k−,0)≜∂xk∂h∣xk=x^k−,vk=0≜∂vk∂f∣xk=x^k−,vk=0
下面是Part2部分的链接，欢迎阅读。
滤波算法_扩展卡尔曼滤波(EKF, Extended Kalman filter)_全网最详细的数学推导_Part2
感谢您的阅读，欢迎留言讨论、收藏、点赞、分享。