卡尔曼增益

卡尔曼增益的直观解释

让我们先来看看一维状态更新方程：

\[ \hat{x}_{n,n} = \hat{x}_{n,n-1} + {k}_{n}({z}_{n} - \hat{x}_{n,n-1}) \]

该方程展示了如何通过使用新息（测量值 \( {z}_{n} \) 与先验估计值之差）调整先验估计值 \( \hat{x}_{n,n-1} \) 来获得更新后的状态估计值 \( \hat{x}_{n,n} \)。卡尔曼增益 \( {k}_{n} \) 决定了我们对测量值和先验估计值的信任程度。

扩展到多变量情况

在一维情况下，系统状态 \( x_{n} \) 和测量值 \( z_{n} \) 处于同一域中。例如，如果 \( x_{n} \) 代表建筑物的真实高度，那么 \( z_{n} \) 就是该高度的直接测量值。

在多变量情况下，情况略有不同。测量向量 \( \boldsymbol{{z}}_{n} \) 和系统状态向量 \( \boldsymbol{x}_{n} \) 通常存在于不同的域中。因此，我们不能直接相减来计算新息。相反，我们必须使用观测矩阵 \( \boldsymbol{H} \) 将系统状态投影到测量域中：

\[ \text{innovation} = (\boldsymbol{{z}}_{n} - \boldsymbol{H}\boldsymbol{\hat{x}}_{n,n-1}) \]

正如在测量方程一章中所讨论的，观测矩阵 \( \boldsymbol{H} \) 将系统状态向量 \( \boldsymbol{{x}}_{n} \) 从状态域映射到测量域。

例如，考虑一枚垂直发射的火箭，其状态由高度 \( h_{n} \) 和垂直速度 \( v_{h_{n}} \) 定义。系统状态向量为：

\[ x_n = \begin{bmatrix} h_n \\ v_{h_n} \end{bmatrix} \]

假设只测量高度。在这种情况下，测量值 \( \boldsymbol{z}_{n} \) 与系统状态的关系为：

\[ \boldsymbol{z}_{n} = \boldsymbol{H} \boldsymbol{x}_{n} + v_n = \begin{bmatrix} 1 & 0 \end{bmatrix} \begin{bmatrix} h_n \\ v_{h_n} \end{bmatrix} + v_n \]

其中：

• \( \boldsymbol{H} = \begin{bmatrix} 1 & 0 \end{bmatrix} \)
• \( v_{n} \) 是测量噪声。

现在让我们仔细看看多维情况下的状态更新方程，正如在状态更新一章中所讨论的。

\[ \boldsymbol{\hat{x}}_{n,n} = \boldsymbol{\hat{x}}_{n,n-1} + \boldsymbol{K}_{n}(\boldsymbol{{z}}_{n} - \boldsymbol{H}\boldsymbol{\hat{x}}_{n,n-1}) \]

由于系统状态 \( \boldsymbol{\hat{x}}_{n,n-1} \) 和测量值 \( \boldsymbol{z}_{n} \) 可能位于不同的域中，我们使用观测矩阵 \( \boldsymbol{H} \) 将状态估计投影到测量域中。

让我们看看一维情况下的卡尔曼增益，其推导过程在一维卡尔曼滤波一章中：

\[ k_{n} = \frac{p_{n,n-1}}{p_{n,n-1} + r_{n}} \]

其中：

• \( p_{n,n-1} \) 是先验估计 \( \hat{x}_{n,n-1} \) 的方差
• \( r_{n} \) 是测量噪声的方差

这个表达式表明，卡尔曼增益 \( k_{n} \) 平衡了先验估计的不确定性和测量的不确定性。当先验估计更不确定时（即 \( p_{n,n-1} \) 很大），增益会将更多的权重转移给测量值。相反，当测量值噪声很大时（即 \( r_{n} \) 很大），增益更倾向于先验估计。

为了使滤波器最优，我们的目标是最小化当前（后验）估计 \( \hat{x}_{n,n} \) 的方差 \( p_{n,n} \)，该估计是基于预测（先验）估计 \( \hat{x}_{n,n-1} \) 和测量值 \( \hat{z}_{n} \) 计算得出的。卡尔曼增益 \( k_{n} \) 最小化了这个后验方差。详细推导在一维卡尔曼滤波一章中提供。

让我们将一维卡尔曼增益重写如下：

\[ k_{n} = p_{n,n-1}(p_{n,n-1} + r_{n})^{-1} \]

在多变量情况下，我们要处理先验估计不确定性（协方差）矩阵 \( \boldsymbol{P}_{n,n-1} \) 和测量噪声不确定性（协方差）矩阵 \( \boldsymbol{R}_{n} \)。在将 \( \boldsymbol{P}_{n,n-1} \) 与测量噪声协方差 \( \boldsymbol{R}_{n} \) 结合之前，必须将其投影到测量域中。这种投影通过观测矩阵 \( \boldsymbol{H} \) 实现。

由于 \( \boldsymbol{P}_{n,n-1} \) 是协方差矩阵，而方差是平方项，因此必须使用以下变换进行投影：

\[ \boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T} \]

（参见本书第 7.5 章中的证明。）

该项表示预测测量的不确定性，类似于一维情况下的 \( p_{n,n-1} \)。

将先验估计不确定性投影到测量域后，多变量情况下的卡尔曼增益* 形式如下：

\[ \color{green}{\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}}(\color{green}{\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}} + \color{blue}{\boldsymbol{R}_{n}})^{-1} \]

（* 这不是卡尔曼增益的最终形式）

卡尔曼增益* 乘以新息为：

\[ \color{green}{\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}}(\color{green}{\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}} + \color{blue}{\boldsymbol{R}_{n}})^{-1}(\boldsymbol{{z}}_{n} - \boldsymbol{H}\boldsymbol{\hat{x}}_{n,n-1}) \]

上述表达式位于测量域中，但在状态更新方程中，我们需要更新位于状态域中的系统状态估计 \( \boldsymbol{\hat{x}}_{n,n-1} \)：

\[ \boldsymbol{\hat{x}}_{n,n} = \boldsymbol{\hat{x}}_{n,n-1} + \boldsymbol{K}_{n}(\boldsymbol{{z}}_{n} - \boldsymbol{H}\boldsymbol{\hat{x}}_{n,n-1}) \]

通过从卡尔曼增益* 表达式中去掉前面的 \( \boldsymbol{H} \)，我们将结果移回状态域。更新后的卡尔曼增益乘以新息的表达式变为：

\[ \color{green}{\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}}(\color{green}{\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}} + \color{blue}{\boldsymbol{R}_{n}})^{-1}(\boldsymbol{{z}}_{n} - \boldsymbol{H}\boldsymbol{\hat{x}}_{n,n-1}) \]

现在我们可以用完整的多变量形式表达状态更新方程：

\[ \boldsymbol{\hat{x}}_{n,n} = \boldsymbol{\hat{x}}_{n,n-1} + \color{red}{\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T} + \boldsymbol{R}_{n})^{-1}}(\boldsymbol{{z}}_{n} - \boldsymbol{H}\boldsymbol{\hat{x}}_{n,n-1}) \]

相应的卡尔曼增益的最终形式为：

\[ \color{red}{\boldsymbol{K}_{n} = \boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T} + \boldsymbol{R}_{n})^{-1}} \]

这种形式确保了新息项正确地从测量域映射回状态域，从而实现系统状态估计的一致且最优的更新。

本节提供了多变量卡尔曼增益的直观理解。下一节将展示其正式推导。

卡尔曼增益方程推导

本章进行卡尔曼增益的详细推导。如果对推导不感兴趣的话可以跳到下一章。

首先，我们来重新写一下协方差更新方程：