其他分享
首页 > 其他分享> > 扩张卡尔曼滤波(EKF)的推导

扩张卡尔曼滤波(EKF)的推导

作者:互联网

卡尔曼滤波是最优线性估计器,然而现实世界中被估计过程和对过程的测量绝大多数都是非线性的,这个时候就要用到EKF。

EKF的思想是使用泰勒展开来对当前的估计状态进行局部线性化。假设过程有一个状态向量 x ∈ R n x\in R^{n} x∈Rn,有一个测量量 z ∈ R m z\in R^{m} z∈Rm该过程可用如下的非线性方程表示:
x k = f ( x k − 1 , u k , w k − 1 )   ( 1 ) z k = h ( x k , v k )   ( 2 ) x_{k} = f(x_{k-1},u_{k},w_{k-1})\space(1) \newline z_{k} = h(x_{k},v_{k})\space (2) xk​=f(xk−1​,uk​,wk−1​) (1)zk​=h(xk​,vk​) (2)
其中:

实际上,没有人能够知道噪声 w k , v k w_{k},v_{k} wk​,vk​在每一个时间点上的值究竟是什么,所以我们将状态和测量向量近似如下:
x ~ k = f ( x ^ k − 1 , u k , 0 )   ( 3 ) z ~ k = h ( x ~ k , 0 )   ( 4 ) \tilde{x}_{k} = f(\hat{x}_{k-1},u_{k},0)\space (3) \newline \tilde{z}_{k} = h(\tilde{x}_{k},0)\space (4) x~k​=f(x^k−1​,uk​,0) (3)z~k​=h(x~k​,0) (4)
其中:

我们对方程(3)和(4)在 x k − 1 x_{k-1} xk−1​处使用泰勒展开,并忽略其二阶及二阶以上的导数,可得如下方程:
x k ≈ x ~ k + A ( x k − 1 − x ^ k − 1 ) + W w k − 1   ( 5 ) z k ≈ z ~ k + H ( x k − x ~ k ) + V v k   ( 6 ) x_{k} \approx \tilde{x}_{k}+A(x_{k-1}-\hat{x}_{k-1})+Ww_{k-1}\space (5) \newline z_{k} \approx \tilde{z}_{k}+H(x_{k}-\tilde{x}_{k})+Vv_{k}\space (6) xk​≈x~k​+A(xk−1​−x^k−1​)+Wwk−1​ (5)zk​≈z~k​+H(xk​−x~k​)+Vvk​ (6)
其中:

定义预测误差为:
e ~ x k = x k − x ~ k   ( 7 ) \tilde{e}_{x_{k}} = x_{k}-\tilde{x}_{k}\space (7) e~xk​​=xk​−x~k​ (7)
定义测量残差为:
e ~ z k = z k − z ~ k   ( 8 ) \tilde{e}_{z_{k}} = z_{k}-\tilde{z}_{k}\space (8) e~zk​​=zk​−z~k​ (8)
其中:

将方程(5)和(6)代入方程(7)和(8),可得到:
e ~ x k ≈ A ( x k − 1 − x ^ k − 1 ) + ε k e ~ z k ≈ H e ~ x k + η k \tilde{e}_{x_{k}} \approx A(x_{k-1}-\hat{x}_{k-1}) + \varepsilon_{k} \newline \tilde{e}_{z_{k}} \approx H \tilde{e}_{x_{k}}+\eta_{k} e~xk​​≈A(xk−1​−x^k−1​)+εk​e~zk​​≈He~xk​​+ηk​
其中 ε k ∼ N ( 0 , W Q k W T ) \varepsilon_{k}\sim N(0,WQ_{k}W^{T}) εk​∼N(0,WQk​WT), η k ∼ N ( 0 , V R k V T ) \eta_{k}\sim N(0,VR_{k}V^{T}) ηk​∼N(0,VRk​VT)
x ^ k = x ~ k + e ^ k e ^ k = K k e ^ z k \hat{x}_{k}=\tilde{x}_{k}+\hat{e}_{k} \newline \hat{e}_{k} = K_{k}\hat{e}_{z_{k}} x^k​=x~k​+e^k​e^k​=Kk​e^zk​​
故而EKF预测方程为:
x ^ k − = f ( x ^ k − 1 , u k , 0 ) P k − = A k P k − 1 A k T + W k Q k − 1 W k T \hat{x}^{-}_{k} = f(\hat{x}_{k-1},u_{k},0) \newline P^{-}_{k}=A_{k}P_{k-1}A^{T}_{k}+W_{k}Q_{k-1}W^{T}_{k} x^k−​=f(x^k−1​,uk​,0)Pk−​=Ak​Pk−1​AkT​+Wk​Qk−1​WkT​
EKF校正方程为:
K k = P k − H k T ( H k P k − H k T + V k R k V k T ) − 1 x ^ k = x ^ k − + K k ( z k − z ~ k ) P k = ( I − K k H k ) P k − K_{k}=P^{-}_{k}H^{T}_{k}(H_{k}P^{-}_{k}H^{T}_{k}+V_{k}R_{k}V^{T}_{k})^{-1} \newline \hat{x}_{k}=\hat{x}^{-}_{k}+K_{k}(z_{k}-\tilde{z}_{k}) \newline P_{k}=(I-K_{k}H_{k})P^{-}_{k} Kk​=Pk−​HkT​(Hk​Pk−​HkT​+Vk​Rk​VkT​)−1x^k​=x^k−​+Kk​(zk​−z~k​)Pk​=(I−Kk​Hk​)Pk−​

参考文献:

[1]Welch G, Bishop G. An introduction to the Kalman filter[J]. 1995.

标签:xk,newline,推导,zk,EKF,卡尔曼滤波,space,tilde,hat
来源: https://blog.csdn.net/weixin_44458490/article/details/117217874