协方差外插方程

无过程噪声的估计协方差

假设过程噪声是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} \) 会造成速度和位置产生对应的扰动。这种情况下，过程噪声在状态变量间就是存在互相关的。

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

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

离散噪声模型

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

对匀速运动模型，过程噪声协方差具有下述形式：

\[ \boldsymbol{Q} = \left[ \begin{matrix} V(x) & COV(x,v) \\ COV(v,x) & V(v) \\ \end{matrix} \right] \]

我们以模型的随机加速度方差 \( \sigma^{2}_{a} \) 来表示位置和速度的协方差。

我们用“必要的背景知识 II”一节里的期望的代数运算法则推导出每个矩阵元素的值：

\[ V(v) = \sigma^{2}_{v} = E\left(v^{2}\right) - \mu_{v}^{2} = E \left( \left( a\Delta t\right)^{2}\right) - \left(\mu_{a}\Delta t\right)^{2} = \Delta t^{2}\left( E\left(a^{2}\right) - \mu_{a}^{2} \right) = \Delta t^{2}\sigma^{2}_{a} \]

\[ V(x) = \sigma^{2}_{x} = E\left(x^{2}\right) - \mu_{x}^{2} = E \left( \left( \frac{1}{2}a\Delta t^{2}\right)^{2}\right) - \left(\frac{1}{2}\mu_{a}\Delta t^{2}\right)^{2} = \frac{\Delta t^{4}}{4}\left( E\left(a^{2}\right) - \mu_{a}^{2} \right) = \frac{\Delta t^{4}}{4}\sigma^{2}_{a} \]

\[ COV(x,v) = COV(v,x) = E\left(xv\right) - \mu_{x}\mu_{v} = E\left( \frac{1}{2}a\Delta t^{2}a\Delta t\right) - \left( \frac{1}{2}\mu_{a}\Delta t^{2}\mu_{a}\Delta t\right) = \frac{\Delta t^{3}}{2}\left( E\left(a^{2}\right) - \mu_{a}^{2} \right) = \frac{\Delta t^{3}}{2}\sigma^{2}_{a} \]

现在我们可以把结果填入 \( \boldsymbol{Q} \) 矩阵：

\[ \boldsymbol{Q} = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{4}}{4} & \frac{\Delta t^{3}}{2} \\ \frac{\Delta t^{3}}{2} & \Delta t^{2} \\ \end{matrix} \right] \]

还有两种方式能快速构建 \( \boldsymbol{Q} \) 矩阵。

状态转移矩阵投影法

如果动态模型不包含输入，我们可以用状态转移矩阵把加速度的方差 \( \sigma^{2}_{a} \) 投影到我们的动态模型中。

我们先定义一个矩阵 \( \boldsymbol{Q}_{a} \)：

\[ \boldsymbol{Q}_{a} = \left[ \begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 1 \\ \end{matrix} \right] \sigma^{2}_{a} \]

过程噪声矩阵为：

\[ \boldsymbol{Q} = \boldsymbol{F}\boldsymbol{Q}_{a}\boldsymbol{F}^{T} \]

对上述运动模型，\( \boldsymbol{F} \) 矩阵为：

\[ \boldsymbol{F} = \left[ \begin{matrix} 1 & \Delta t & \frac{\Delta t^{2}}{2} \\ 0 & 1 & \Delta t \\ 0 & 0 & 1 \\ \end{matrix} \right] \]

\[ \boldsymbol{Q} = \boldsymbol{F}\boldsymbol{Q}_{a}\boldsymbol{F}^{T} = \]

\[ = \left[ \begin{matrix} 1 & \Delta t & \frac{\Delta t^{2}}{2} \\ 0 & 1 & \Delta t \\ 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} 1 & 0 & 0 \\ \Delta t & 1 & 0 \\ \frac{\Delta t^{2}}{2} & \Delta t & 1 \\ \end{matrix} \right] \sigma^{2}_{a} = \]

\[ = \left[ \begin{matrix} 0 & 0 & \frac{\Delta t^{2}}{2} \\ 0 & 0 & \Delta t \\ 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} 1 & 0 & 0 \\ \Delta t & 1 & 0 \\ \frac{\Delta t^{2}}{2} & \Delta t & 1 \\ \end{matrix} \right] \sigma^{2}_{a} = \]

\[ = \left[ \begin{matrix} \frac{\Delta t^{4}}{4} & \frac{\Delta t^{3}}{2} & \frac{\Delta t^{2}}{2} \\ \frac{\Delta t^{3}}{2} & \Delta t^{2} & \Delta t \\ \frac{\Delta t^{2}}{2} & \Delta t & 1 \\ \end{matrix} \right] \sigma^{2}_{a} \]

输入转移矩阵投影法

如果动态模型包含输入，我们可以更快捷地算出 \( \boldsymbol{Q} \) 矩阵。我们可以用输入转移矩阵把加速度方差 \( \sigma^{2}_{a} \) 投影到我们的动态模型里。

\[ \boldsymbol{Q} = \boldsymbol{G}\sigma^{2}_{a}\boldsymbol{G^{T}} \]

式中 \( \boldsymbol{G} \) 是控制矩阵（或输入转移矩阵）。

对上述运动模型，\( \boldsymbol{G} \) 矩阵为：

\[ \boldsymbol{G} = \left[ \begin{matrix} \frac{\Delta t^{2}}{2} \\ \Delta t \\ \end{matrix} \right] \]

\[ \boldsymbol{Q} = \boldsymbol{G}\sigma^{2}_{a}\boldsymbol{G^{T}} = \sigma^{2}_{a}\boldsymbol{G}\boldsymbol{G^{T}} = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{2}}{2} \\ \Delta t \\ \end{matrix} \right] \left[ \begin{matrix} \frac{\Delta t^{2}}{2} & \Delta t \\ \end{matrix} \right] = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{4}}{4} & \frac{\Delta t^{3}}{2} \\ \frac{\Delta t^{3}}{2} & \Delta t^{2} \\ \end{matrix} \right] \]

你可以用上述方法构建离散 \( \boldsymbol{Q} \) 矩阵。

连续噪声模型

连续噪声模型假设噪声随时间连续改变。

为了推导连续噪声模型的过程噪声协方差矩阵 \( \boldsymbol{Q}_{c} \)，我们需要把离散过程噪声的协方差矩阵 \( \boldsymbol{Q} \) 对时间进行积分。

\[ \boldsymbol{Q}_{c} = \int _{0}^{ \Delta t}\boldsymbol{Q}dt = \int _{0}^{ \Delta t} \sigma^{2}_{a} \left[ \begin{matrix} \frac{t^{4}}{4} & \frac{t^{3}}{2} \\ \frac{t^{3}}{2} & t^{2} \\ \end{matrix} \right] dt = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{5}}{20} & \frac{\Delta t^{4}}{8} \\ \frac{\Delta t^{4}}{8} & \frac{\Delta t^{3}}{3} \\ \end{matrix} \right] \]

选择哪种噪声模型？

回答这个问题之前，你需要选择合适的过程噪声方差。你可以用统计和随机领域的公式去计算，或者根据经验手动选择一个合适的值（这种比较推荐）。

在雷达领域，\( \sigma^{2}_{a} \) 依赖目标的特性和模型完整性。对可以机动的目标，比如飞机，\( \sigma^{2}_{a} \) 应该相对高；对不能机动的目标，比如火箭，可以用较小的 \( \sigma^{2}_{a} \). 模型完整性也是选择过程噪声方差的因素之一。如果你的模型包含了环境扰动，例如空气阻力，那么过程噪声的比例会比不包含要小。（译注：目标特性和模型完整性是一回事，一句话说就是过程噪声就是模型不准确性，模型近似程度越高就越不准确，噪声方差就要选得越大，大到一定程度KF就只相信测量值了，就起不到滤波效果了。）

一旦选定了过程噪声方差的值，接下来就要选择噪声模型了。应该选离散的还是连续的？

这个问题没有明确的答案。我推荐两种模型都尝试一下，实测看看哪种模型在你的滤波器中效果最好。当 \( \Delta t \) 非常小时，可以选取离散噪声模型，反之则最好选用连续噪声模型。