协方差更新方程

协方差更新方程为：

P_{n, n} = (I - K_{n} H) P_{n, n - 1} {(I - K_{n} H)}^{T} + K_{n} R_{n} K_{n}^{T}

式中：

$P_{n, n}$	是当前状态估计的协方差矩阵
$P_{n, n - 1}$	是前一时刻对当前状态的预测的协方差矩阵
$K_{n}$	是卡尔曼增益
$H$	是观测矩阵
$R_{n}$	是测量噪声的协方差矩阵
$I$	是单位阵（一个 $n \times n$ 且仅有对角元素为1、其余元素均为0的方阵）

Kalman Filter Book

协方差更新方程推导

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

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

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

公式编号	公式	注释
1	${\hat{x}}_{n, n} = {\hat{x}}_{n, n - 1} + K_{n} (z_{n} - {H \hat{x}}_{n, n - 1})$	状态更新方程
2	$z_{n} = {H x}_{n} + v_{n}$	测量方程
3	$P_{n, n} = E (e_{n} e_{n}^{T}) = E ((x_{n} - {\hat{x}}_{n, n}) {(x_{n} - {\hat{x}}_{n, n})}^{T})$	估计协方差
4	$R_{n} = E (v_{n} v_{n}^{T})$	测量协方差

我们以卡尔曼增益 $K_{n}$ 为自变量来推导当前估计的协方差 $P_{n, n}$ .

	注解
${\hat{x}}_{n, n} = {\hat{x}}_{n, n - 1} + K_{n} (z_{n} - {H \hat{x}}_{n, n - 1})$	状态更新方程
${\hat{x}}_{n, n} = {\hat{x}}_{n, n - 1} + K_{n} ({H x}_{n} + v_{n} - {H \hat{x}}_{n, n - 1})$	代入测量方程
$e_{n} = x_{n} - {\hat{x}}_{n, n}$	测量误差
$e_{n} = x_{n} - {\hat{x}}_{n, n - 1} - K_{n} ({H x}_{n} + v_{n} - {H \hat{x}}_{n, n - 1})$	代入 ${\hat{x}}_{n, n}$
$e_{n} = x_{n} - {\hat{x}}_{n, n - 1} - K_{n} {H x}_{n} - K_{n} v_{n} + K_{n} {H \hat{x}}_{n, n - 1}$	展开
$e_{n} = x_{n} - {\hat{x}}_{n, n - 1} - K_{n} H (x_{n} - {\hat{x}}_{n, n - 1}) - K_{n} v_{n}$	围绕 $(x_{n} - {\hat{x}}_{n, n - 1})$ 进行整理
$e_{n} = (I - K_{n} H) (x_{n} - {\hat{x}}_{n, n - 1}) - K_{n} v_{n}$	把 $(x_{n} - {\hat{x}}_{n, n - 1})$ 提出来
$P_{n, n} = E (e_{n} e_{n}^{T}) = E ((x_{n} - {\hat{x}}_{n, n}) {(x_{n} - {\hat{x}}_{n, n})}^{T})$	这是估计的协方差（根据协方差方程）
$P_{n, n} = E (((I - K_{n} H) (x_{n} - {\hat{x}}_{n, n - 1}) - K_{n} v_{n}) \times \times {((I - K_{n} H) (x_{n} - {\hat{x}}_{n, n - 1}) - K_{n} v_{n})}^{T})$	代入 $e_{n}$
$P_{n, n} = E (((I - K_{n} H) (x_{n} - {\hat{x}}_{n, n - 1}) - K_{n} v_{n}) \times \times ({((I - K_{n} H) (x_{n} - {\hat{x}}_{n, n - 1}))}^{T} - {(K_{n} v_{n})}^{T}))$	转置号分配进去
$P_{n, n} = E (((I - K_{n} H) (x_{n} - {\hat{x}}_{n, n - 1}) - K_{n} v_{n}) \times \times ({(x_{n} - {\hat{x}}_{n, n - 1})}^{T} {(I - K_{n} H)}^{T} - {(K_{n} v_{n})}^{T}))$	应用矩阵转置性质： $(A B)^{T} = B^{T} A^{T}$
$P_{n, n} = E ((I - K_{n} H) (x_{n} - {\hat{x}}_{n, n - 1}) {(x_{n} - {\hat{x}}_{n, n - 1})}^{T} {(I - K_{n} H)}^{T} - - (I - K_{n} H) (x_{n} - {\hat{x}}_{n, n - 1}) {(K_{n} v_{n})}^{T} - - K_{n} v_{n} {(x_{n} - {\hat{x}}_{n, n - 1})}^{T} {(I - K_{n} H)}^{T} + + K_{n} v_{n} {(K_{n} v_{n})}^{T})$	展开
$P_{n, n} = E ((I - K_{n} H) (x_{n} - {\hat{x}}_{n, n - 1}) {(x_{n} - {\hat{x}}_{n, n - 1})}^{T} {(I - K_{n} H)}^{T}) - - E ((I - K_{n} H) (x_{n} - {\hat{x}}_{n, n - 1}) {(K_{n} v_{n})}^{T}) - - E (K_{n} v_{n} {(x_{n} - {\hat{x}}_{n, n - 1})}^{T} {(I - K_{n} H)}^{T}) + + E (K_{n} v_{n} {(K_{n} v_{n})}^{T})$	应用期望性质： $E (X \pm Y) = E (X) \pm E (Y)$
$E ((I - K_{n} H) (x_{n} - {\hat{x}}_{n, n - 1}) {(K_{n} v_{n})}^{T}) = 0$ $E (K_{n} v_{n} {(x_{n} - {\hat{x}}_{n, n - 1})}^{T} {(I - K_{n} H)}^{T}) = 0$	$(x_{n} - {\hat{x}}_{n, n - 1})$ 是先验估计的误差，它和当前测量噪声 $v_{n}$ 无关。两个不相关的随机变量的积的期望是0.
$P_{n, n} = E ((I - K_{n} H) (x_{n} - {\hat{x}}_{n, n - 1}) {(x_{n} - {\hat{x}}_{n, n - 1})}^{T} {(I - K_{n} H)}^{T}) + + E (K_{n} v_{n} v_{n}^{T} K_{n}^{T})$	应用矩阵转置性质： $(A B)^{T} = B^{T} A^{T}$
$P_{n, n} = (I - K_{n} H) E ((x_{n} - {\hat{x}}_{n, n - 1}) {(x_{n} - {\hat{x}}_{n, n - 1})}^{T}) {(I - K_{n} H)}^{T} + + K_{n} E (v_{n} v_{n}^{T}) K_{n}^{T}$	应用期望性质： $E (a X) = a E (X)$
$E ((x_{n} - {\hat{x}}_{n, n - 1}) {(x_{n} - {\hat{x}}_{n, n - 1})}^{T}) = P_{n, n - 1}$ $E (v_{n} v_{n}^{T}) = R_{n}$	$P_{n, n - 1}$ 是先验估计的协方差 $R_{n}$ 是测量协方差
$P_{n, n} = (I - K_{n} H) P_{n, n - 1} {(I - K_{n} H)}^{T} + K_{n} R_{n} K_{n}^{T}$	就得到了协方差更新方程！

上一章下一章