ICode9

精准搜索请尝试: 精确搜索
首页 > 编程语言> 文章详细

卡尔曼滤波算法公式详解及推导

2021-04-16 12:33:00  阅读:377  来源: 互联网

标签:推导 cov Kk 卡尔曼滤波 tr Hk 详解 Pk hat


卡尔曼滤波原理及实践

卡尔曼滤波公式详解

已知假设,卡尔曼滤波模型假设k时刻的真实状态是从(k − 1)时刻的状态演化而来,符合下式:
x k = F k x k − 1 + B k u k + w k x_{k} = F_k x_{k-1} + B_ku_k + w_k xk​=Fk​xk−1​+Bk​uk​+wk​

  • F k F_k Fk​为状态变换模型(矩阵/向量),运动学一般是矩阵(状态转移矩阵)。
  • B k B_k Bk​是作用在控制器向量 u k u_k uk​上的输入-控制模型。一般运动学中没有这项。
  • w k ∼ N ( 0 , Q k ) w_k \sim N(0,Q_k) wk​∼N(0,Qk​)是过程噪声。均值为0,可不写这项。

k时刻真实状态 x k x_k xk​的一个测量:
z k = H k ∗ x k + v k z_k = H_k * x_k + v_k zk​=Hk​∗xk​+vk​

  • H k H_k Hk​为观测模型(观测矩阵),它把真实状态空间映射成观测空间, v k ∼ N ( 0 , R k ) v_k \sim N(0,R_k) vk​∼N(0,Rk​)是观测噪声
    初始状态以及每一时刻的噪声 x 0 , w 1 , . . . , w k , v 1... v k {x_0, w_1, ..., w_k, v1 ... v_k} x0​,w1​,...,wk​,v1...vk​都认为是互相独立的

预测:

x ^ k ∣ k − 1 = F k x ^ k − 1 ∣ k − 1 \hat x_{k|k-1} = F_k \hat x_{k-1|k-1} x^k∣k−1​=Fk​x^k−1∣k−1​

  • 预测状态,状态方程。
  • x ^ k ∣ k − 1 \hat x_{k|k-1} x^k∣k−1​为k-1时刻对k时刻的状态预测。
  • x ^ k − 1 ∣ k − 1 \hat x_{k-1|k-1} x^k−1∣k−1​在时刻k-1的状态估计。
  • F k F_k Fk​为状态转移矩阵

P k ∣ k − 1 = F k P k − 1 ∣ k − 1 F k T + Q k P_{k|k-1} = F_k P_{k-1|k-1} F^T_k + Q_k Pk∣k−1​=Fk​Pk−1∣k−1​FkT​+Qk​

  • 预测估计协方差矩阵,先验概率
  • P k − 1 ∣ k − 1 P_{k-1|k-1} Pk−1∣k−1​为k-1时刻的后验估计误差协方差矩阵,度量估计值的精确程度。
  • P k ∣ k − 1 P_{k|k-1} Pk∣k−1​为k-1时刻到k时刻的估计误差协方差矩阵。
  • Q k Q_k Qk​为过程噪声协方差矩阵,越大说明越不相信预测。实际工程中过程噪声由厂商提供,也可以自己调参。

更新:

K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1 K_k = P_{k|k-1}H^T_k {(H_k P_{k|k-1} H^T_k + R_k)}^{-1} Kk​=Pk∣k−1​HkT​(Hk​Pk∣k−1​HkT​+Rk​)−1

  • K k K_k Kk​为最优卡尔曼增益
  • R k R_k Rk​为测量噪声协方差矩阵,越大说明越不相信观测。实际工程中可自己调参,也可以自适应(AKF)。
    x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) \hat x_{k|k} = \hat x_{k|k-1} + K_k (z_k - H_k \hat x_{k|k-1}) x^k∣k​=x^k∣k−1​+Kk​(zk​−Hk​x^k∣k−1​)
  • 更新状态估计
  • 可以使用测量残差来简化表达公式 y ^ = z k − H k x ^ k ∣ k − 1 \hat y = z_k - H_k \hat x_{k|k-1} y^​=zk​−Hk​x^k∣k−1​, S k = c o v ( y ^ ) = H k P k ∣ k − 1 H k T + R k S_k = cov(\hat y) = H_k P_{k|k-1} H^T_k + R_k Sk​=cov(y^​)=Hk​Pk∣k−1​HkT​+Rk​
  • 即卡尔曼增益可以简化为 K k = P k ∣ k − 1 H k T S k − 1 K_k = P_{k|k-1}H^T_k S_k^{-1} Kk​=Pk∣k−1​HkT​Sk−1​,状态估计更新可以简化为 x ^ k ∣ k = x ^ k ∣ k − 1 + K k y ^ \hat x_{k|k} = \hat x_{k|k-1} + K_k \hat y x^k∣k​=x^k∣k−1​+Kk​y^​
    P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k|k} = (I - K_k H_k) P_{k|k-1} Pk∣k​=(I−Kk​Hk​)Pk∣k−1​
  • 更新协方差估计,后验概率

公式推导

已知: Q k Q_k Qk​和 R k R_k Rk​一般为固定值,高级卡尔曼滤波可以用自适应。
P k ∣ k − 1 = c o v ( x k − x ^ k ∣ k − 1 ) = E ( ( x k − x ^ k ∣ k − 1 ) ( x k − x ^ k ∣ k − 1 ) T ) P_{k|k-1} = cov(x_k - \hat x_{k|k-1}) = E((x_k - \hat x_{k|k-1})(x_k - \hat x_{k|k-1})^T) Pk∣k−1​=cov(xk​−x^k∣k−1​)=E((xk​−x^k∣k−1​)(xk​−x^k∣k−1​)T)
P k ∣ k = c o v ( x k − x ^ k ∣ k ) = E ( ( x k − x ^ k ∣ k ) ( x k − x ^ k ∣ k ) T ) P_{k|k} = cov(x_k - \hat x_{k|k}) = E((x_k - \hat x_{k|k})(x_k - \hat x_{k|k})^T) Pk∣k​=cov(xk​−x^k∣k​)=E((xk​−x^k∣k​)(xk​−x^k∣k​)T)
S k = c o v ( y ^ ) = c o v ( z k − H k x ^ k ∣ k − 1 ) S_k = cov(\hat y) = cov(z_k - H_k \hat x_{k|k-1}) Sk​=cov(y^​)=cov(zk​−Hk​x^k∣k−1​)

  • 从状态方程推 P k ∣ k − 1 P_{k|k-1} Pk∣k−1​:

P k ∣ k − 1 = c o v ( x k − x ^ k ∣ k − 1 ) P_{k|k-1} = cov(x_k - \hat x_{k|k-1}) Pk∣k−1​=cov(xk​−x^k∣k−1​)
P k ∣ k − 1 = c o v ( F k x k − 1 + w k − F k x ^ k − 1 ∣ k − 1 ) P_{k|k-1} = cov(F_k x_{k-1} + w_k - F_k \hat x_{k-1|k-1}) Pk∣k−1​=cov(Fk​xk−1​+wk​−Fk​x^k−1∣k−1​)
P k ∣ k − 1 = c o v ( F k ( x k − 1 − x ^ k − 1 ∣ k − 1 ) ) + c o v ( w k ) P_{k|k-1} = cov(F_k (x_{k-1}-\hat x_{k-1|k-1})) + cov(w_k) Pk∣k−1​=cov(Fk​(xk−1​−x^k−1∣k−1​))+cov(wk​)
P k ∣ k − 1 = F k c o v ( x k − 1 − x ^ k − 1 ∣ k − 1 ) F k T + Q k P_{k|k-1} = F_k cov(x_{k-1}-\hat x_{k-1|k-1})F_k^T + Q_k Pk∣k−1​=Fk​cov(xk−1​−x^k−1∣k−1​)FkT​+Qk​
P k ∣ k − 1 = F k P k − 1 ∣ k − 1 F k T + Q k P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k Pk∣k−1​=Fk​Pk−1∣k−1​FkT​+Qk​

  • 从状态估计更新方程推 P k ∣ k P_{k|k} Pk∣k​:

P k ∣ k = c o v ( x k − x ^ k ∣ k ) P_{k|k} = cov(x_k - \hat x_{k|k}) Pk∣k​=cov(xk​−x^k∣k​)
P k ∣ k = c o v ( x k − x ^ k ∣ k − 1 − K k ( z k − H k x ^ k ∣ k − 1 ) ) P_{k|k} = cov(x_k - \hat x_{k|k-1} - K_k (z_k - H_k \hat x_{k|k-1})) Pk∣k​=cov(xk​−x^k∣k−1​−Kk​(zk​−Hk​x^k∣k−1​))
P k ∣ k = c o v ( x k − x ^ k ∣ k − 1 − K k ( H k x k + v k − H k x ^ k ∣ k − 1 ) ) P_{k|k} = cov(x_k - \hat x_{k|k-1} - K_k (H_k x_k + v_k - H_k \hat x_{k|k-1})) Pk∣k​=cov(xk​−x^k∣k−1​−Kk​(Hk​xk​+vk​−Hk​x^k∣k−1​))
P k ∣ k = c o v ( x k − x ^ k ∣ k − 1 − K k H k ( x k − x ^ k ∣ k − 1 ) − K k v k ) P_{k|k} = cov(x_k - \hat x_{k|k-1} - K_k H_k( x_k - \hat x_{k|k-1}) - K_k v_k) Pk∣k​=cov(xk​−x^k∣k−1​−Kk​Hk​(xk​−x^k∣k−1​)−Kk​vk​)
P k ∣ k = c o v ( ( I − K k H k ) ( x k − x ^ k ∣ k − 1 ) − K k v k ) P_{k|k} = cov((I - K_k H_k)( x_k - \hat x_{k|k-1}) - K_k v_k) Pk∣k​=cov((I−Kk​Hk​)(xk​−x^k∣k−1​)−Kk​vk​)
P k ∣ k = c o v ( ( I − K k H k ) ( x k − x ^ k ∣ k − 1 ) ) + c o v ( K k v k ) P_{k|k} = cov((I - K_k H_k)( x_k - \hat x_{k|k-1})) + cov(K_k v_k) Pk∣k​=cov((I−Kk​Hk​)(xk​−x^k∣k−1​))+cov(Kk​vk​)
P k ∣ k = ( I − K k H k ) c o v ( x k − x ^ k ∣ k − 1 ) ( I − K k H k ) T + K k c o v ( v k ) K k T P_{k|k} = (I - K_k H_k)cov( x_k - \hat x_{k|k-1})(I - K_k H_k)^T + K_k cov(v_k) K_k^T Pk∣k​=(I−Kk​Hk​)cov(xk​−x^k∣k−1​)(I−Kk​Hk​)T+Kk​cov(vk​)KkT​
P k ∣ k = ( I − K k H k ) P k ∣ k − 1 ( I − K k H k ) T + K k R k K k T P_{k|k} = (I - K_k H_k) P_{k|k-1} (I - K_k H_k)^T + K_k R_k K_k^T Pk∣k​=(I−Kk​Hk​)Pk∣k−1​(I−Kk​Hk​)T+Kk​Rk​KkT​

这一公式对于任何卡尔曼增益 K k {K_k} Kk​都成立。如果 K k {K_k} Kk​是最优卡尔曼增益,则可以进一步简化

  • 基于上式推导最优卡尔曼增益 K k {K_k} Kk​:

卡尔曼滤波器是最小均方误差估计器,后验状态误差估计是指 P k ∣ k = c o v ( x k − x ^ k ∣ k ) P_{k|k} = cov(x_k - \hat x_{k|k}) Pk∣k​=cov(xk​−x^k∣k​),当 P k ∣ k P_{k|k} Pk∣k​矩阵的均方误差为最小时,即可求出最优卡尔曼增益。

矩阵的均方误差为矩阵的迹。求矩阵的最小均方误差,即是求矩阵的迹的最小值,对矩阵的迹求导,导数为0时,迹最小。矩阵的迹求导有相关公式,网上查。协方差 P k ∣ k P_{k|k} Pk∣k​是对称矩阵。

P k ∣ k = ( I − K k H k ) P k ∣ k − 1 ( I − K k H k ) T + K k R k K k T P_{k|k} = (I - K_k H_k) P_{k|k-1} (I - K_k H_k)^T + K_k R_k K_k^T Pk∣k​=(I−Kk​Hk​)Pk∣k−1​(I−Kk​Hk​)T+Kk​Rk​KkT​

P k ∣ k = ( P k ∣ k − 1 − K k H k P k ∣ k − 1 ) ( I − ( K k H k ) T ) + K k R k K k T P_{k|k} = (P_{k|k-1} - K_k H_k P_{k|k-1})(I - (K_k H_k)^T) + K_k R_k K_k^T Pk∣k​=(Pk∣k−1​−Kk​Hk​Pk∣k−1​)(I−(Kk​Hk​)T)+Kk​Rk​KkT​

P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + K k H k P k ∣ k − 1 ( K k H k ) T + K k R k K k T P_{k|k} = P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + K_k H_k P_{k|k-1}(K_k H_k)^T + K_k R_k K_k^T Pk∣k​=Pk∣k−1​−Kk​Hk​Pk∣k−1​−Pk∣k−1​(Kk​Hk​)T+Kk​Hk​Pk∣k−1​(Kk​Hk​)T+Kk​Rk​KkT​

P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + K k ( H k P k ∣ k − 1 H k T + R k ) K k T P_{k|k} = P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T Pk∣k​=Pk∣k−1​−Kk​Hk​Pk∣k−1​−Pk∣k−1​(Kk​Hk​)T+Kk​(Hk​Pk∣k−1​HkT​+Rk​)KkT​

t r ( P k ∣ k ) = t r ( P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + + K k ( H k P k ∣ k − 1 H k T + R k ) K k T ) tr(P_{k|k}) = tr(P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + + K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T) tr(Pk∣k​)=tr(Pk∣k−1​−Kk​Hk​Pk∣k−1​−Pk∣k−1​(Kk​Hk​)T++Kk​(Hk​Pk∣k−1​HkT​+Rk​)KkT​)

t r ( P k ∣ k ) = t r ( P k ∣ k − 1 ) − t r ( K k H k P k ∣ k − 1 ) − t r ( P k ∣ k − 1 ( K k H k ) T ) + t r ( K k ( H k P k ∣ k − 1 H k T + R k ) K k T ) tr(P_{k|k}) = tr(P_{k|k-1} )- tr(K_k H_k P_{k|k-1}) -tr( P_{k|k-1} (K_k H_k)^T) + tr( K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T) tr(Pk∣k​)=tr(Pk∣k−1​)−tr(Kk​Hk​Pk∣k−1​)−tr(Pk∣k−1​(Kk​Hk​)T)+tr(Kk​(Hk​Pk∣k−1​HkT​+Rk​)KkT​)

t r ( P k ∣ k ) = t r ( P k ∣ k − 1 ) − 2 t r ( K k H k P k ∣ k − 1 ) + t r ( K k ( H k P k ∣ k − 1 H k T + R k ) K k T ) tr(P_{k|k}) = tr(P_{k|k-1} )- 2tr(K_k H_k P_{k|k-1})+ tr( K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T) tr(Pk∣k​)=tr(Pk∣k−1​)−2tr(Kk​Hk​Pk∣k−1​)+tr(Kk​(Hk​Pk∣k−1​HkT​+Rk​)KkT​)

d   t r ( P k ∣ k ) d   K k = d   [ t r ( P k ∣ k − 1 ) − 2 t r ( K k H k P k ∣ k − 1 ) + t r ( K k ( H k P k ∣ k − 1 H k T + R k ) K k T ) ] d   K k \frac {d \ tr(P_{k|k})} {d\ K_k} =\frac {d\ {[tr(P_{k|k-1}) - 2tr(K_k H_k P_{k|k-1}) + tr(K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T)]}} {d\ K_k} d Kk​d tr(Pk∣k​)​=d Kk​d [tr(Pk∣k−1​)−2tr(Kk​Hk​Pk∣k−1​)+tr(Kk​(Hk​Pk∣k−1​HkT​+Rk​)KkT​)]​

d   t r ( P k ∣ k ) d   K k = − 2 ( H k P k ∣ k − 1 ) T + 2 K k ( H k P k ∣ k − 1 H k T + R k ) \frac {d \ tr(P_{k|k})} {d\ K_k} =-2 (H_k P_{k|k-1})^T + 2K_k(H_k P_{k|k-1} H^T_k + R_k) d Kk​d tr(Pk∣k​)​=−2(Hk​Pk∣k−1​)T+2Kk​(Hk​Pk∣k−1​HkT​+Rk​)

当矩阵导数是0的时候得到 P k ∣ k P_{k|k} Pk∣k​的迹(trace)的最小值:
t r ( P k ∣ k ) d K k = − 2 ( H k P k ∣ k − 1 ) T + 2 K k ( H k P k ∣ k − 1 H k T + R k ) = 0 \frac {tr(P_{k|k})} {dK_k} =-2 (H_k P_{k|k-1})^T + 2K_k(H_k P_{k|k-1} H^T_k + R_k) = 0 dKk​tr(Pk∣k​)​=−2(Hk​Pk∣k−1​)T+2Kk​(Hk​Pk∣k−1​HkT​+Rk​)=0

K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1 = P k ∣ k − 1 H k T S k − 1 K_k = P_{k|k-1}H^T_k {(H_k P_{k|k-1} H^T_k + R_k)}^{-1} = P_{k|k-1}H^T_k S_k^{-1} Kk​=Pk∣k−1​HkT​(Hk​Pk∣k−1​HkT​+Rk​)−1=Pk∣k−1​HkT​Sk−1​

  • 最优卡尔曼增益化简 P k ∣ k P_{k|k} Pk∣k​

K k S k = ( H k P k ∣ k − 1 ) T K_kS_k = (H_k P_{k|k-1})^T Kk​Sk​=(Hk​Pk∣k−1​)T

K k S k K k T = ( H k P k ∣ k − 1 ) T K k T = P k ∣ k − 1 ( K k H k ) T K_k S_k K^T_k = (H_k P_{k|k-1})^T K^T_k = P_{k|k-1} (K_k H_k)^T Kk​Sk​KkT​=(Hk​Pk∣k−1​)TKkT​=Pk∣k−1​(Kk​Hk​)T

P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + K k ( H k P k ∣ k − 1 H k T + R k ) K k T P_{k|k} = P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T Pk∣k​=Pk∣k−1​−Kk​Hk​Pk∣k−1​−Pk∣k−1​(Kk​Hk​)T+Kk​(Hk​Pk∣k−1​HkT​+Rk​)KkT​

P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + K k S k K k T P_{k|k} = P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + K_k S_k K^T_k Pk∣k​=Pk∣k−1​−Kk​Hk​Pk∣k−1​−Pk∣k−1​(Kk​Hk​)T+Kk​Sk​KkT​

P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + P k ∣ k − 1 ( K k H k ) T P_{k|k} = P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + P_{k|k-1} (K_k H_k)^T Pk∣k​=Pk∣k−1​−Kk​Hk​Pk∣k−1​−Pk∣k−1​(Kk​Hk​)T+Pk∣k−1​(Kk​Hk​)T

P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 P_{k|k} = P_{k|k-1} - K_k H_k P_{k|k-1} Pk∣k​=Pk∣k−1​−Kk​Hk​Pk∣k−1​

P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k|k} = (I - K_k H_k )P_{k|k-1} Pk∣k​=(I−Kk​Hk​)Pk∣k−1​

该简化式 P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k|k} = (I - K_k H_k )P_{k|k-1} Pk∣k​=(I−Kk​Hk​)Pk∣k−1​只能在最优卡尔曼增益时才成立。如果算术精度总是很低而导致数值稳定性出现问题,或者特意使用非最优卡尔曼增益,则必须使用上面未简化后的后验误差协方差公式 P k ∣ k = ( I − K k H k ) P k ∣ k − 1 ( I − K k H k ) T + K k R k K k T P_{k|k} = (I - K_k H_k) P_{k|k-1} (I - K_k H_k)^T + K_k R_k K_k^T Pk∣k​=(I−Kk​Hk​)Pk∣k−1​(I−Kk​Hk​)T+Kk​Rk​KkT​。

标签:推导,cov,Kk,卡尔曼滤波,tr,Hk,详解,Pk,hat
来源: https://blog.csdn.net/weixin_43152152/article/details/115753921

本站声明: 1. iCode9 技术分享网(下文简称本站)提供的所有内容,仅供技术学习、探讨和分享;
2. 关于本站的所有留言、评论、转载及引用,纯属内容发起人的个人观点,与本站观点和立场无关;
3. 关于本站的所有言论和文字,纯属内容发起人的个人观点,与本站观点和立场无关;
4. 本站文章均是网友提供,不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属;如您发现该文章侵犯了您的权益,可联系我们第一时间进行删除;
5. 本站为非盈利性的个人网站,所有内容不会用来进行牟利,也不会利用任何形式的广告来间接获益,纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

专注分享技术,共同学习,共同进步。侵权联系[81616952@qq.com]

Copyright (C)ICode9.com, All Rights Reserved.

ICode9版权所有