订购

协方差更新方程

协方差更新方程为：

\[ \boldsymbol{P}_{n,n} = \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \boldsymbol{P}_{n,n-1} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} + \boldsymbol{K}_{n}\boldsymbol{R}_{n}\boldsymbol{K}_{n}^{T} \]
式中：
\( \boldsymbol{P}_{n,n} \) 是当前状态估计的协方差矩阵
\( \boldsymbol{P}_{n,n-1} \) 是前一时刻对当前状态的预测的协方差矩阵
\( \boldsymbol{K}_{n} \) 是卡尔曼增益
\( \boldsymbol{H} \) 是观测矩阵
\( \boldsymbol{R}_{n} \) 是测量噪声的协方差矩阵
\( \boldsymbol{I} \) 是单位阵（一个 \( n \times n \) 且仅有对角元素为1、其余元素均为0的方阵）
Kalman Filter Book
获取本书

协方差更新方程推导

本节给出协方差更新方程的推导。一些读者可能觉得叙述过于细节化了，但这对其他还不熟悉的人来说很有帮助。

如果对推导不感兴趣，你可以跳到下一章。

我用下面四个公式来进行推导：

公式编号 公式 注释
1 \( \boldsymbol{\hat{x}}_{n,n} = \boldsymbol{\hat{x}}_{n,n-1} + \boldsymbol{K}_{n} ( \boldsymbol{z}_{n} - \boldsymbol{H \hat{x}}_{n,n-1} ) \) 状态更新方程
2 \( \boldsymbol{z}_{n} = \boldsymbol{Hx}_{n} + \boldsymbol{v}_{n} \) 测量方程
3 \( \boldsymbol{P}_{n,n} = E\left( \boldsymbol{e}_{n}\boldsymbol{e}_{n}^{T} \right) = E\left( \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n} \right)^{T} \right) \) 估计协方差
4 \( \boldsymbol{R}_{n} = E\left( \boldsymbol{v}_{n}\boldsymbol{v}_{n}^{T} \right) \) 测量协方差

我们以卡尔曼增益 \( \boldsymbol{K}_{n} \) 为自变量来推导当前估计的协方差 \( \boldsymbol{P}_{n,n} \).

注解
\( \boldsymbol{\hat{x}}_{n,n} = \boldsymbol{\hat{x}}_{n,n-1} + \boldsymbol{K}_{n} ( \boldsymbol{z}_{n} - \boldsymbol{H \hat{x}}_{n,n-1} ) \) 状态更新方程
\( \boldsymbol{\hat{x}}_{n,n} = \boldsymbol{\hat{x}}_{n,n-1} + \boldsymbol{K}_{n} ( \boldsymbol{Hx}_{n} + \boldsymbol{v}_{n} - \boldsymbol{H \hat{x}}_{n,n-1} ) \) 代入测量方程
\( \boldsymbol{e}_{n} = \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n} \) 测量误差
\( \boldsymbol{e}_{n} = \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} - \boldsymbol{K}_{n} \left(\boldsymbol{Hx}_{n} + \boldsymbol{v}_{n} - \boldsymbol{H \hat{x}}_{n,n-1} \right) \) 代入 \( \boldsymbol{\hat{x}}_{n,n} \)
\( \boldsymbol{e}_{n} = \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} - \boldsymbol{K}_{n}\boldsymbol{Hx}_{n} - \boldsymbol{K}_{n}\boldsymbol{v}_{n} + \boldsymbol{K}_{n}\boldsymbol{H \hat{x}}_{n,n-1} \) 展开
\( \boldsymbol{e}_{n} = \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} - \boldsymbol{K}_{n}\boldsymbol{H}\left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \) 围绕 \( (\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1}) \) 进行整理
\( \boldsymbol{e}_{n} = \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n}\) 把 \( (\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1}) \) 提出来
\( \boldsymbol{P}_{n,n} = E\left( \boldsymbol{e}_{n}\boldsymbol{e}_{n}^{T} \right) = E\left( \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n} \right)^{T} \right) \) 这是估计的协方差（根据协方差方程）
\( \boldsymbol{P}_{n,n} = E\left( \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right) \times \\ \times \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} \right) \) 代入 \( \boldsymbol{e}_{n} \)
\( \boldsymbol{P}_{n,n} = E\left( \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right) \times \\ \times \left( \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \right)^{T} - \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n}\right) ^{T} \right) \right) \) 转置号分配进去
\( \boldsymbol{P}_{n,n} = E\left(\left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right) \times \\ \times \left( \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} - \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n}\right)^{T} \right) \right) \) 应用矩阵转置性质：\( (\boldsymbol{AB})^{T} = \boldsymbol{B}^{T}\boldsymbol{A}^{T} \)
\( \boldsymbol{P}_{n,n} = E \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} - \\ - \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} - \\ - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} + \\ + \boldsymbol{K}_{n}\boldsymbol{v}_{n} \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} \right) \) 展开
\( \boldsymbol{P}_{n,n} = E \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} \right) - \\ - \color{red}{E \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} \right)} - \\ - \color{red}{E \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} \right)} + \\ + E \left( \color{blue}{\boldsymbol{K}_{n}\boldsymbol{v}_{n} \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} }\right) \) 应用期望性质：\( E(X \pm Y) = E(X) \pm E(Y) \)
\( \color{red}{E \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} \right) = 0} \)
\( \color{red}{E \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} \right) = 0} \) 		\( (\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1}) \) 是先验估计的误差，它和当前测量噪声 \( \boldsymbol{v}_{n} \) 无关。两个不相关的随机变量的积的期望是0.
\( \boldsymbol{P}_{n,n} = E \left( \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} \right) + \\ + E \left( \color{blue}{\boldsymbol{K}_{n}\boldsymbol{v}_{n}\boldsymbol{v}_{n}^{T}\boldsymbol{K}_{n}^{T}}\right) \) 应用矩阵转置性质：\( (\boldsymbol{AB})^{T} = \boldsymbol{B}^{T}\boldsymbol{A}^{T} \)
\( \boldsymbol{P}_{n,n} = \boldsymbol{ \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)} \color{green}{E \left( \left(\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left(\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \right)} \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} + \\ + \boldsymbol{K}_{n} \color{blue}{ E \left( \boldsymbol{v}_{n}\boldsymbol{v}_{n}^{T} \right) } \boldsymbol{ K}_{n}^{T} \) 应用期望性质：\( E(aX) = aE(X) \)
\( \color{green}{E \left( \left(\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left(\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \right) = \boldsymbol{P}_{n,n-1}} \)
\( \color{blue}{ E \left( \boldsymbol{v}_{n}\boldsymbol{v}_{n}^{T}\right) = \boldsymbol{R}_{n}} \) 		\( \color{green}{\boldsymbol{P}_{n,n-1}} \) 是先验估计的协方差
\( \color{blue}{\boldsymbol{R}_{n}} \) 是测量协方差
\( \boldsymbol{P}_{n,n} = \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \color{green}{\boldsymbol{P}_{n,n-1} } \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} + \boldsymbol{K}_{n} \color{blue}{ \boldsymbol{R}_{n}} \boldsymbol{K}_{n}^{T} \) 就得到了协方差更新方程！
上一章 下一章