协方差外插方程

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

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

P_{n + 1, n} = {F P}_{n, n} F^{T} + Q

其中：

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

获取本书

无过程噪声的估计协方差

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

P_{n + 1, n} = {F P}_{n, n} F^{T}

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

C O V (x) = E ((x - μ_{x}) {(x - μ_{x})}^{T})

向量 $x$ 是系统状态向量。

因此：

P_{n, n} = E (({\hat{x}}_{n, n} - μ_{x_{n, n}}) {({\hat{x}}_{n, n} - μ_{x_{n, n}})}^{T})

根据状态外插方程：

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

因此：

P_{n + 1, n} = E (({\hat{x}}_{n + 1, n} - μ_{x_{n + 1, n}}) {({\hat{x}}_{n + 1, n} - μ_{x_{n + 1, n}})}^{T}) =

= E (({F \hat{x}}_{n, n} + {G \hat{u}}_{n, n} - {F μ_{x}}_{n, n} - {G \hat{u}}_{n, n}) {({F \hat{x}}_{n, n} + {G \hat{u}}_{n, n} - {F μ_{x}}_{n, n} - {G \hat{u}}_{n, n})}^{T}) =

= E (F ({\hat{x}}_{n, n} - μ_{x_{n, n}}) {(F ({\hat{x}}_{n, n} - μ_{x_{n, n}}))}^{T}) =

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

= E (F ({\hat{x}}_{n, n} - μ_{x_{n, n}}) {({\hat{x}}_{n, n} - μ_{x_{n, n}})}^{T} F^{T}) =

= F E (({\hat{x}}_{n, n} - μ_{x_{n, n}}) {({\hat{x}}_{n, n} - μ_{x_{n, n}})}^{T}) F^{T} =

= F P_{n, n} F^{T}

构建过程噪声矩阵 $Q$

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

{\hat{x}}_{n + 1, n} = {F \hat{x}}_{n, n} + {G \hat{u}}_{n, n} + w_{n}

式中 $w_{n}$ 是 $n$ 时刻的过程噪声。

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

在多维情况下，过程噪声是一个协方差矩阵，记为 $Q$ .

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

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

Q = [\begin{matrix} q_{11} & 0 & \dots & 0 \\ 0 & q_{22} & \dots & 0 \\ ⋮ & ⋮ & ⋱ & ⋮ \\ 0 & 0 & \dots & q_{k k} \end{matrix}]

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

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

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

离散噪声模型

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

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

Q = [\begin{matrix} V (x) & C O V (x, v) \\ C O V (v, x) & V (v) \end{matrix}]

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

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

V (v) = σ_{v}^{2} = E (v^{2}) - μ_{v}^{2} = E ({(a Δ t)}^{2}) - {(μ_{a} Δ t)}^{2} = Δ t^{2} (E (a^{2}) - μ_{a}^{2}) = Δ t^{2} σ_{a}^{2}

V (x) = σ_{x}^{2} = E (x^{2}) - μ_{x}^{2} = E ({(\frac{1}{2} a Δ t^{2})}^{2}) - {(\frac{1}{2} μ_{a} Δ t^{2})}^{2} = \frac{Δ t^{4}}{4} (E (a^{2}) - μ_{a}^{2}) = \frac{Δ t^{4}}{4} σ_{a}^{2}

C O V (x, v) = C O V (v, x) = E (x v) - μ_{x} μ_{v} = E (\frac{1}{2} a Δ t^{2} a Δ t) - (\frac{1}{2} μ_{a} Δ t^{2} μ_{a} Δ t) = \frac{Δ t^{3}}{2} (E (a^{2}) - μ_{a}^{2}) = \frac{Δ t^{3}}{2} σ_{a}^{2}

现在我们可以把结果填入 $Q$ 矩阵：

Q = σ_{a}^{2} [\begin{matrix} \frac{Δ t^{4}}{4} & \frac{Δ t^{3}}{2} \\ \frac{Δ t^{3}}{2} & Δ t^{2} \end{matrix}]

还有两种方式能快速构建 $Q$ 矩阵。

状态转移矩阵投影法

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

我们先定义一个矩阵 $Q_{a}$ ：

Q_{a} = [\begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 1 \end{matrix}] σ_{a}^{2}

过程噪声矩阵为：

Q = F Q_{a} F^{T}

对上述运动模型， $F$ 矩阵为：

F = [\begin{matrix} 1 & Δ t & \frac{Δ t^{2}}{2} \\ 0 & 1 & Δ t \\ 0 & 0 & 1 \end{matrix}]

Q = F Q_{a} F^{T} =

= [\begin{matrix} 1 & Δ t & \frac{Δ t^{2}}{2} \\ 0 & 1 & Δ t \\ 0 & 0 & 1 \end{matrix}] [\begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 \\ Δ t & 1 & 0 \\ \frac{Δ t^{2}}{2} & Δ t & 1 \end{matrix}] σ_{a}^{2} =

= [\begin{matrix} 0 & 0 & \frac{Δ t^{2}}{2} \\ 0 & 0 & Δ t \\ 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 \\ Δ t & 1 & 0 \\ \frac{Δ t^{2}}{2} & Δ t & 1 \end{matrix}] σ_{a}^{2} =

= [\begin{matrix} \frac{Δ t^{4}}{4} & \frac{Δ t^{3}}{2} & \frac{Δ t^{2}}{2} \\ \frac{Δ t^{3}}{2} & Δ t^{2} & Δ t \\ \frac{Δ t^{2}}{2} & Δ t & 1 \end{matrix}] σ_{a}^{2}

输入转移矩阵投影法

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

Q = G σ_{a}^{2} G^{T}

式中 $G$ 是控制矩阵（或输入转移矩阵）。

对上述运动模型， $G$ 矩阵为：

G = [\begin{matrix} \frac{Δ t^{2}}{2} \\ Δ t \end{matrix}]

Q = G σ_{a}^{2} G^{T} = σ_{a}^{2} G G^{T} = σ_{a}^{2} [\begin{matrix} \frac{Δ t^{2}}{2} \\ Δ t \end{matrix}] [\begin{matrix} \frac{Δ t^{2}}{2} & Δ t \end{matrix}] = σ_{a}^{2} [\begin{matrix} \frac{Δ t^{4}}{4} & \frac{Δ t^{3}}{2} \\ \frac{Δ t^{3}}{2} & Δ t^{2} \end{matrix}]

你可以用上述方法构建离散 $Q$ 矩阵。

连续噪声模型

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

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

Q_{c} = \int_{0}^{Δ t} Q d t = \int_{0}^{Δ t} σ_{a}^{2} [\begin{matrix} \frac{t^{4}}{4} & \frac{t^{3}}{2} \\ \frac{t^{3}}{2} & t^{2} \end{matrix}] d t = σ_{a}^{2} [\begin{matrix} \frac{Δ t^{5}}{20} & \frac{Δ t^{4}}{8} \\ \frac{Δ t^{4}}{8} & \frac{Δ t^{3}}{3} \end{matrix}]

选择哪种噪声模型？

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

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

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

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