卡尔曼学习(6)

卡尔曼滤波中的误差协方差矩阵计算与卡尔曼增益求解

原创已于 2024-03-14 16:17:41 修改 · 623 阅读

8 ·

CC 4.0 BY-SA版权

文章标签：

#学习 #算法

于 2024-03-14 16:05:27 首次发布

卡尔曼滤波专栏收录该内容

6 篇文章

订阅专栏

本文详细介绍了卡尔曼滤波中的关键步骤，包括误差协方差矩阵的计算，以及如何利用先验估计、后验估计和卡尔曼增益进行预测、校正和更新过程。通过公式推导，总结了卡尔曼滤波的五个核心公式。

误差协方差矩阵计算:

前面我们已经得到了

$X_k = AX_{k-1}+BU_{k-1}+w_{k-1}$ (1)

$Z_k = HX_{k}+v_k$ (2)

$P(w) \sim (0,Q)$

$P(v) \sim (0,R)$

先验估计： $\hat{X}_{k}^- = A\hat{X}_{k-1} + BU_{k-1}$ (3)

后验估计： $\hat{X}_k = \hat{X}_{k}^- + K_k(Z_k - H\hat{X}_{k}^-)$ (4)

卡尔曼增益： $K_k = \frac{P_k^-H^T}{HP_k^-H^T + R}$ (5)

公式（5）中的 $P_k^- = E[e_k^-e_k^{-T}]$ 是误差协防矩阵，其他的都是已知的，现在要计算卡尔曼增益就需要求解 $P_k^-$ 。

将 $e_k = X_k - \hat{X}_{k}^-\\=AX_{k-1}+BU_{k-1}+w_{k-1} - ( A\hat{X}_{k-1} + BU_{k-1})\\=A(X_{k-1}-\hat{X}_{k-1})+w_{k-1}\\=Ae_{k-1}+w_{k-1}$ 代入 $P_k^- = E[e_k^-e_k^{-T}]$ 得:

$P_k^- = E[e_k^-e_k^{-T}]\\=E[(Ae_{k-1}+w_{k-1})(Ae_{k-1}+w_{k-1})^T]\\=E[(Ae_{k-1}+w_{k-1})(e_{k-1}^TA^T+w_{k-1}^T)]\\=E[Ae_{k-1}e_{k-1}^TA^T+Ae_{k-1}w_{k-1}^T+w_{k-1}e_{k-1}^TA^T+w_{k-1}w_{k-1}^T]\\=E[Ae_{k-1}e_{k-1}^TA^T]+E[Ae_{k-1}w_{k-1}^T]+E[w_{k-1}e_{k-1}^TA^T]+E[w_{k-1}w_{k-1}^T]\\=AE[e_{k-1}e_{k-1}^T]A^T+AE[e_{k-1}]E[w_{k-1}^T]+E[w_{k-1}]E[e_{k-1}^T]A^T+E[w_{k-1}w_{k-1}^T]$ (6)

因为 $e_k$ 和 $w_k$ 期望为0，又因为 $P_{k-1}=E[e_{k-1}e_{k-1}^T]$ 和 $Q= E[w_{k-1}w_{k-1}^T]$ ，所以

$P_{k}^- = AP_{k-1}A^T+Q$

由之前的卡尔曼学习(5)中公式(12)可知 $P_k = P_k^--K_kHP_k^--P_k^-H^TK_k^T+K_kHP_k^-H^TK_k^T+K_kR_kK_k^T$

整理上式，并将公式(5)代入:

$P_k = P_k^--K_kHP_k^--P_k^-H^TK_k^T+K_k(HP_k^-H^T+R_k)K_k^T\\=P_k^--K_kHP_k^--P_k^-H^TK_k^T+\frac{P_k^-H^T}{HP_k^-H^T+R}(HP_k^-H^T+R_k)K_k^T\\=P_k^--K_kHP_k^--P_k^-H^TK_k^T+P_k^-H^TK_k^T\\=P_k^--K_kHP_k^-\\=(I-K_kH)P_k^-$

自此我们得到卡尔曼滤波的所有公式：

预测：

先验值： $\hat{X}_{k}^- = A\hat{X}_{k-1} + BU_{k-1}$

先验误差的协方差： $P_{k}^- = AP_{k-1}A^T+Q$

校正：

卡尔曼增益： $K_k = \frac{P_k^-H^T}{HP_k^-H^T+R}$

后验估计： $\hat{X}_k = \hat{X}_{k}^- + K_k(Z_k - H\hat{X}_{k}^-)$

更新先验误差的协方差： $P_k =(I-K_kH)P_k^-$

学习笔记，参考资料【【卡尔曼滤波器】4_误差协方差矩阵数学推导_卡尔曼滤波器的五个公式】 https://www.bilibili.com/video/BV1yV411B7DM/?p=4&share_source=copy_web&vd_source=c29456ffa88bc7559f8ffbe6f8e8f7a5