025卡尔曼滤波中滤波增益与协方差阵的等价形式

最新推荐文章于 2025-04-23 15:08:06 发布

魔方的块

最新推荐文章于 2025-04-23 15:08:06 发布

阅读量1.6k

点赞数 6

CC 4.0 BY-SA版权

分类专栏：算法文章标签：卡尔曼滤波增益协方差

本文链接：https://blog.youkuaiyun.com/Pro2015/article/details/82908148

算法专栏收录该内容

2 篇文章

订阅专栏

本文详细推导了卡尔曼滤波器的关键公式，包括滤波增益和协方差矩阵的等价表达式，展示了如何通过矩阵求逆引理简化计算过程。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

首先摆放一下前面的求解结果：
$\tag{1} K_k = P_{k/k-1} H_k^T(H_k P_{k/k-1} H_k^T + R_k)^{-1}$ $\tag{2} P_k = (I - K_k H_k ) P_{k/k-1}$ $P_k = (I - K_kH_k) P_{k/k-1} (I - K_kH_k)^T + K_k R_k K_k^T\\$

公式(1)代入公式(2)展开的：
$\tag{3} \begin{aligned} P_k &= P_{k/k-1} - P_{k/k-1} H_k^T(H_k P_{k/k-1} H_k^T + R_k)^{-1} H_k P_{k/k-1} \\ \\ &= P_{k/k-1} - P_{k/k-1} H_k^T( R_k + H_k P_{k/k-1} H_k^T )^{-1} H_k P_{k/k-1} \\ \end{aligned}$

比较矩阵求逆引理：
$A+BCD)^{-1} = A^{-1} - A^{-1}B(C^{-1}+DA^{-1}B)^{-1} DA^{-1}$
可将公式(3)转换为：

$\tag{4} P_k = (P_{k/k-1}^{-1} + H_k^T R_k^{-1} H_k) ^{-1}$

前面已求得：
$P_{k/k-1} H_k^T = K_k(H_k P_{k/k-1} H_k^T + R_k)$
两边转置：
$H_k P_{k/k-1} = (H_k P_{k/k-1} H_k^T + R_k)K^T_k$
将公式(2)并将上式代入可得：

$P_k = P_{k/k-1} - K_k (H_k P_{k/k-1} H_k^T + R_k)K^T_k$

根据矩阵求逆引理的方法构造公式(1):

$\tag{5} \begin{aligned} K_k &= P_{k/k-1} H_k^T(H_k P_{k/k-1} H_k^T + R_k)^{-1}\\ \\ &= [(H_k P_{k/k-1} H_k^T + R_k)(H_k^T)^{-1} P_{k/k-1}^{-1}]^{-1}\\ \\ &= [H_k + R_k(H_k^T)^{-1} P_{k/k-1}^{-1}]^{-1}\\ \\ &= [R_k(H_k^T)^{-1}( H_k^T R_k^{-1} H_k + P_{k/k-1}^{-1})]^{-1}\\ \\ &= ( H_k^T R_k^{-1} H_k + P_{k/k-1}^{-1})^{-1}H_k^T R_k^{-1}\\ \end{aligned}$

将公式(4)代入公式(5)：

$K_k = P_k H_k^T R_k^{-1}$

总结一下等价形式，对于滤波增益：

$\begin{aligned} 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 H_k^T R_k^{-1} \end{aligned}$
对于协方差阵：

$\begin{aligned} P_k &= (I - K_k H_k ) P_{k/k-1} \\ \\ P_k &= P_{k/k-1} - K_k (H_k P_{k/k-1} H_k^T + R_k)K^T_k\\ \\ P_k &= (I - K_kH_k) P_{k/k-1} (I - K_kH_k)^T + K_k R_k K_k^T\\ \\ P_k &= (P_{k/k-1}^{-1} + H_k^T R_k^{-1} H_k) ^{-1}\\ \end{aligned}$