卡尔曼滤波器:误差协方差矩阵

丁毅桂2025/1/7大约 5 分钟

卡尔曼滤波器学习笔记

【卡尔曼滤波器】_DR_CAN合集学习笔记

对于如下状态空间方程：

{\begin{cases} x_{k} & = A x_{k - 1} + B u_{k - 1} + w_{k - 1} \\ z_{k} & = H x_{k} + v_{k} \end{cases}

其中包含的过程噪声 $w$ 和测量噪声 $v$ 符合正态分布:

\begin{aligned} w & \sim N (μ, Q) & μ = 0; Q = E (w w^{T}) \\ v & \sim N (μ, R) & μ = 0; R = E (v v^{T}) \end{aligned}

定义先验估计为：

{\hat{x}}_{k}^{-} = A {\hat{x}}_{k - 1} + B u_{k - 1}

后验估计为：

{\hat{x}}_{k} = {\hat{x}}_{k}^{-} + K_{k} (z_{k} - H {\hat{x}}_{k}^{-})

卡尔曼增益为：

K_{k} = \frac{P_{k}^{-} H^{T}}{H P_{k}^{-} H^{T} + R}

上述所有公式中，唯有先验估计误差的方差 $P_{K}^{-}$ 是未知的，于是根据公式可依次推导：

\begin{aligned} e_{k}^{-} & = x_{k} - x_{k}^{-} \\ = (A x_{k - 1} + B u_{k - 1} + w_{k - 1}) - (A {\hat{x}}_{k - 1} + B u_{k - 1}) & (根据定义代入) \\ = A x_{k - 1} + B u_{k - 1} + w_{k - 1} - A {\hat{x}}_{k - 1} - B u_{k - 1} \\ = A (x_{k - 1} - {\hat{x}}_{k - 1}) + w_{k - 1} \\ = A e_{k - 1} + w_{k - 1} & (估计误差 e = x - \hat{x}) \end{aligned}

所以：

\begin{aligned} P_{K}^{-} & = E (e_{k}^{-} {e_{k}^{-}}^{T}) \\ = E [(A e_{k - 1} + w_{k - 1}) (A e_{k - 1} + w_{k - 1})^{T}] \\ = E [(A e_{k - 1} + w_{k - 1}) (e_{k - 1}^{T} A^{T} + w_{k - 1}^{T})] \\ = E [A e_{k - 1} e_{k - 1}^{T} A^{T} + A e_{k - 1} w_{k - 1}^{T} + w_{k - 1} e_{k - 1}^{T} A^{T} + w_{k - 1} w_{k - 1}^{T}] \\ = E [A e_{k - 1} e_{k - 1}^{T} A^{T}] + E [A e_{k - 1} w_{k - 1}^{T}] + E [w_{k - 1} e_{k - 1}^{T} A^{T}] + E [w_{k - 1} w_{k - 1}^{T}] \\ = 第一项 + 第二项 + 第三项 + 第四项 \end{aligned}

其中

\begin{aligned} 第一项 & = E [A e_{k - 1} e_{k - 1}^{T} A^{T}] \\ = A E [e_{k - 1} e_{k - 1}^{T}] A^{T} \\ = A P_{k - 1} A^{T} \\ 第二项 & = E [A e_{k - 1} w_{k - 1}^{T}] \\ = A E [e_{k - 1} w_{k - 1}^{T}] \\ = A E [(x_{k - 1} - {\hat{x}}_{k - 1}) w_{k - 1}^{T}] & (e_{k - 1} = x_{k - 1} - {\hat{x}}_{k - 1}) \\ = A E [((A x_{k - 2} + B u_{k - 2} + w_{k - 2}) - {\hat{x}}_{k - 1}) w_{k - 1}^{T}] & (x_{k - 1} = A x_{k - 2} + B u_{k - 2} + w_{k - 2}) \\ = A E [e_{k - 1}] E [w_{k - 1}^{T}] & (e_{k - 1} 中包含的是 w_{k - 2},其和 w_{k - 1} 相互独立) \\ = A \times 0 \times 0 \\ = 0 \\ 第三项 & = E [w_{k - 1} e_{k - 1}^{T} A^{T}] \\ = E [w_{k - 1} e_{k - 1}^{T}] A^{T} \\ = E [w_{k - 1}] E [e_{k - 1}^{T}] A^{T} & (e_{k - 1} 中包含的是 w_{k - 2},其和 w_{k - 1} 相互独立) \\ = 0 \times 0 \times A^{T} \\ = 0 \\ 第四项 & = E [w_{k - 1} w_{k - 1}^{T}] \\ = Q_{k - 1} & (代入) \\ = Q & (过程噪声是模型的固有特性，不变) \end{aligned}

所以最终得到：

\begin{aligned} P_{K}^{-} & = 第一项 + 第二项 + 第三项 + 第四项 \\ = A P_{k - 1} A^{T} + 0 + 0 + Q \\ = A P_{k - 1} A^{T} + Q \end{aligned}

其中的后验估计误差协方差 $P_{k - 1}$ 为：

\begin{aligned} P_{k} & = Var (e_{k}) \\ = P_{k}^{-} - P_{k}^{-} H^{T} K_{k}^{T} - K_{k} H P_{k}^{-} + K_{k} H P_{k}^{-} H^{T} K_{k}^{T} + K_{k} R K_{k}^{T} & (之前推导的) \\ = P_{k}^{-} - P_{k}^{-} H^{T} K_{k}^{T} - K_{k} H P_{k}^{-} + K_{k} (H P_{k}^{-} H^{T} + R) K_{k}^{T} & (提取) \\ = P_{k}^{-} - P_{k}^{-} H^{T} K_{k}^{T} - K_{k} H P_{k}^{-} + (\frac{P_{k}^{-} H^{T}}{H P_{k}^{-} H^{T} + R}) (H P_{k}^{-} H^{T} + R) K_{k}^{T} & (代入 K_{k}) \\ = P_{k}^{-} - P_{k}^{-} H^{T} K_{k}^{T} - K_{k} H P_{k}^{-} + (P_{k}^{-} H^{T}) K_{k}^{T} & (乘除抵消) \\ = P_{k}^{-} - K_{k} H P_{k}^{-} & (第二、四项抵消) \\ = (I - K_{k} H) P_{k}^{-} \end{aligned}

状态空间方程：

{\begin{cases} x_{k} & = A x_{k - 1} + B u_{k - 1} + w_{k - 1} \\ z_{k} & = H x_{k} + v_{k} \end{cases}

其中包含的过程噪声 $w$ 和测量噪声 $v$ 符合正态分布:

\begin{aligned} w & \sim N (μ, Q) & μ = 0; Q = E (w w^{T}) \\ v & \sim N (μ, R) & μ = 0; R = E (v v^{T}) \end{aligned}

第一步：求先验估计

不考虑过程噪声，利用上次状态，计算预测本次状态（先验估计）

{\hat{x}}_{k}^{-} = A {\hat{x}}_{k - 1} + B u_{k - 1}

第二步：求先验估计误差（的协方差矩阵）

根据上次后验估计误差（的协方差矩阵）和模型误差（过程噪声的协方差矩阵），求本次先验误差（的协方差矩阵）

P_{K}^{-} = A P_{k - 1} A^{T} + Q

第三步：求卡尔曼增益：

根据估计误差和测量误差，求数据融合的最优系数

K_{k} = \frac{P_{k}^{-} H^{T}}{H P_{k}^{-} H^{T} + R}

第四步：求后验估计：

通过卡尔曼增益，将估计值和测量值数据融合，得到后验最优估计

{\hat{x}}_{k} = {\hat{x}}_{k}^{-} + K_{k} (z_{k} - H {\hat{x}}_{k}^{-})

第五步：求后验估计误差（的协方差矩阵）：

根据本次卡尔曼增益和先验估计误差，求本次后验估计的误差

\begin{array}{r} P_{k} = (I - K_{k} H) P_{k}^{-} \end{array}