卡尔曼增益
最后一个方程是卡尔曼增益方程。
矩阵形式的卡尔曼增益为：
\[ \boldsymbol{K}_{n} = \boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}\left(\boldsymbol{HP}_{n,n-1}\boldsymbol{H}^{T} + \boldsymbol{R}_{n} \right)^{-1} \]
式中：
| \( \boldsymbol{K}_{n} \) | 是卡尔曼增益 |
| \( \boldsymbol{P}_{n,n-1} \) | 是前一时刻对当前状态的预测的协方差矩阵 |
| \( \boldsymbol{H} \) | 是观测矩阵 |
| \( \boldsymbol{R}_{n} \) | 是测量噪声的协方差矩阵 |
卡尔曼增益方程推导
本章进行卡尔曼增益的详细推导。如果对推导不感兴趣的话可以跳到下一章。
首先，我们来重新写一下协方差更新方程：
| 注释 | |
|---|---|
| \( \boldsymbol{P}_{n,n} = \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \boldsymbol{P}_{n,n-1} \color{blue}{\left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T}} + \boldsymbol{K}_{n} \boldsymbol{R}_{n}\boldsymbol{K}_{n}^{T} \) | 协方差更新方程 |
| \( \boldsymbol{P}_{n,n} = \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \boldsymbol{P}_{n,n-1} \color{blue}{\left(\boldsymbol{I} - \left(\boldsymbol{K}_{n}\boldsymbol{H}\right)^{T}\right)} + \boldsymbol{K}_{n} \boldsymbol{R}_{n} \boldsymbol{K}_{n}^{T} \) | \( \boldsymbol{I}^{T} = \boldsymbol{I} \) |
| \( \boldsymbol{P}_{n,n} = \color{green}{\left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \boldsymbol{P}_{n,n-1}} \color{blue}{\left(\boldsymbol{I} - \boldsymbol{H}^{T}\boldsymbol{K}_{n}^{T}\right)} + \boldsymbol{K}_{n} \boldsymbol{R}_{n} \boldsymbol{K}_{n}^{T} \) | 应用矩阵转置性质：\( (\boldsymbol{AB})^{T} = \boldsymbol{B}^{T}\boldsymbol{A}^{T} \) |
| \( \boldsymbol{P}_{n,n} = \color{green}{\left(\boldsymbol{P}_{n,n-1} - \boldsymbol{K}_{n}\boldsymbol{H}\boldsymbol{P}_{n,n-1} \right)} \left(\boldsymbol{I} - \boldsymbol{H}^{T}\boldsymbol{K}_{n}^{T}\right) + \boldsymbol{K}_{n} \boldsymbol{R}_{n} \boldsymbol{K}_{n}^{T} \) | |
| \( \boldsymbol{P}_{n,n} = \boldsymbol{P}_{n,n-1} - \boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}\boldsymbol{K}_{n}^{T} - \boldsymbol{K}_{n}\boldsymbol{H}\boldsymbol{P}_{n,n-1} + \\ + \color{#7030A0}{\boldsymbol{K}_{n}\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}\boldsymbol{K}_{n}^{T} + \boldsymbol{K}_{n} \boldsymbol{R}_{n} \boldsymbol{K}_{n}^{T} } \) | 展开 |
| \( \boldsymbol{P}_{n,n} = \boldsymbol{P}_{n,n-1} - \boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}\boldsymbol{K}_{n}^{T} - \boldsymbol{K}_{n}\boldsymbol{H}\boldsymbol{P}_{n,n-1} + \\ + \color{#7030A0}{\boldsymbol{K}_{n} \left( \boldsymbol{H} \boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T} + \boldsymbol{R}_{n} \right) \boldsymbol{K}_{n}^{T} } \) | 后两项提出 \( \boldsymbol{K}_{n} \)，得到关于 \( \boldsymbol{K}_{n} \) 的二次型 |
卡尔曼滤波是一种最优滤波器，我们希望寻找一个卡尔曼增益，使其最小化估计值的方差。
为了最小化估计值的方差，我们需要最小化协方差矩阵 \( \boldsymbol{P}_{n,n} \) 的主对角线（左上角到右下角）。
方阵的主对角线元素和称为矩阵的迹。因此，我们需要最小化 \( tr(\boldsymbol{P}_{n,n}) \). 为了得到最优化条件，我们把 \( \boldsymbol{P}_{n,n} \) 的迹对卡尔曼增益 \( \boldsymbol{K}_{n} \) 求导，并令导数为0.