协方差外插方程

我想读者应该已经熟悉了协方差外插（预测）。我们在一维卡尔曼滤波中已经讲过协方差外插方程（或协方差预测器方程）。本节中，我们将推导卡尔曼滤波的协方差外插方程的矩阵形式。

协方差外插方程的一般形式如下给出：

\[ \boldsymbol{P}_{n+1,n} = \boldsymbol{FP}_{n,n}\boldsymbol{F}^{T} + \boldsymbol{Q} \]

其中：

\( \boldsymbol{P}_{n,n} \)	是当前状态估计的不确定性的平方（协方差矩阵）
\( \boldsymbol{P}_{n+1,n} \)	是下一个状态预测的不确定性的平方（协方差矩阵）
\( \boldsymbol{F} \)	是在“线性动态系统建模”一节推导的状态转移矩阵
\( \boldsymbol{Q} \)	是过程噪声矩阵

基于示例的卡尔曼滤波指南

获取本书

无过程噪声的估计协方差

假设过程噪声是0 \( (Q=0) \)，那么：

\[ \boldsymbol{P}_{n+1,n} = \boldsymbol{FP}_{n,n}\boldsymbol{F}^{T} \]

上述推导是十分直观的。我已经在"必要的背景知识 II"中讲过：

\[ COV(\boldsymbol{x}) = E \left( \left( \boldsymbol{x - \mu_{x}} \right) \left( \boldsymbol{x - \mu_{x}} \right)^{T} \right) \]

向量 \( x \) 是系统状态向量。

因此：

\[ \boldsymbol{P}_{n,n} = E \left( \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right)^{T} \right) \]

根据 状态外插方程：

\[ \boldsymbol{\hat{x}}_{n+1,n} = \boldsymbol{F\hat{x}}_{n,n} + \boldsymbol{G\hat{u}}_{n,n} \]

因此：

\[ \boldsymbol{P}_{n+1,n} = E \left( \left( \boldsymbol{\hat{x}}_{n+1,n} - \boldsymbol{\mu}_{x_{n+1,n}} \right) \left( \boldsymbol{\hat{x}}_{n+1,n} - \boldsymbol{\mu}_{x_{n+1,n}} \right)^{T} \right) = \]

\[ = E \left( \left( \boldsymbol{F\hat{x}}_{n,n} + \boldsymbol{G\hat{u}}_{n,n} - \boldsymbol{F\mu_{x}}_{n,n} - \boldsymbol{G\hat{u}}_{n,n} \right) \left( \boldsymbol{F\hat{x}}_{n,n} + \boldsymbol{G\hat{u}}_{n,n} - \boldsymbol{F\mu_{x}}_{n,n} - \boldsymbol{G\hat{u}}_{n,n} \right)^{T} \right) = \]

\[ = E \left( \boldsymbol{F} \left( \boldsymbol{\hat{x}}_{n,n} - \boldsymbol{\mu}_{x_{n,n}} \right) \left( \boldsymbol{F} \left( \boldsymbol{\hat{x}}_{n,n} - \boldsymbol{\mu}_{x_{n,n}} \right) \right)^{T} \right) = \]

应用矩阵转置的性质：\( \boldsymbol{(AB)}^T = \boldsymbol{B}^T \boldsymbol{A}^T \)

\[ = E \left(\boldsymbol{F} \left( \boldsymbol{\hat{x}}_{n,n} - \boldsymbol{\mu}_{x_{n,n}} \right) \left( \boldsymbol{\hat{x}}_{n,n} - \boldsymbol{\mu}_{x_{n,n}} \right)^{T} \boldsymbol{F}^{T} \right) = \]

\[ = \boldsymbol{F} E \left( \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right)^{T} \right) \boldsymbol{F}^{T} = \]

\[ = \boldsymbol{F} \boldsymbol{P}_{n,n} \boldsymbol{F}^{T} \]

构建过程噪声矩阵 \( Q \)

你已经知道系统动态模型是这样描述的：

\[ \boldsymbol{\hat{x}}_{n+1,n} = \boldsymbol{F\hat{x}}_{n,n} + \boldsymbol{G\hat{u}}_{n,n} + \boldsymbol{w}_{n} \]

式中 \( \boldsymbol{w}_{n} \) 是 \( n \) 时刻的过程噪声。

我们已经在“一维卡尔曼滤波”一节中讨论了过程噪声和它对卡尔曼滤波性能的影响。在一维卡尔曼滤波中，过程噪声方差记为 \( q \).

在多维情况下，过程噪声是一个协方差矩阵，记为 \( \boldsymbol{Q} \).

我们已经见到过程噪声方差对卡尔曼滤波性能有非常大的影响。过小的 \( q \) 会造成滞后误差（见示例 7）。而如果 \( q \) 的值过高，卡尔曼滤波会完全相信测量值（见示例 8），造成估计值噪声过大。

不同状态变量对应的过程噪声可以是独立的。这种情况下，过程噪声协方差矩阵 \( \boldsymbol{Q} \) 是对角阵：

\[ \boldsymbol{Q} = \left[ \begin{matrix} q_{11} & 0 & \cdots & 0 \\ 0 & q_{22} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & q_{kk} \\ \end{matrix} \right] \]

过程噪声也可以是相关的。比如匀速运动模型假设零加速度（\(a=0\)），但实际加速度的随机噪声 \( \sigma^{2}_{a} \) 会造成速度和位置产生对应的扰动。这种情况下，过程噪声在状态变量间就是存在互相关的。

环境过程噪声有两种模型：

• 离散噪声模型
• 连续噪声模型

离散噪声模型

离散噪声模型假设噪声在每个采样点是不同的，但是在采样点之间是相同的（零阶保持）。