卡尔曼增益

最后一个方程是卡尔曼增益方程。

矩阵形式的卡尔曼增益为:

\[ \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.

上一章 下一章