深入浅出卡尔曼滤波

0. 写在开头

#

点进这篇博客的读者应该都是对卡尔曼滤波有所兴趣的，或者在工作中遇到卡尔曼滤波的问题，这里我就不介绍卡尔曼滤波的背景了，直接介绍最基础的卡尔曼滤波（KF）。

卡尔曼滤波，用直白的话来讲，就是：多个不确定的结果，经过分析、推理和计算，获得相对准确的结果。

• 多个是指数据来源可以是模型推理得出，也可以是通过仪器测量获得。
• 不确定是指由于模型本身是一种近似，或者是测量仪器本身的精度误差，或者测量过程不可避免地引入了噪声，甚至因为所需要的特征无法直接获取，只能间接推导获得。
• 分析、推理和计算，则指的是卡尔曼滤波算法，也是本文接下来将会重点阐述的部分。
• 相对准确，指的是经过卡尔曼滤波算法获得的结果，比原有的多个不确定的结果更逼近客观真实值，但依然存在误差。

原理

#

数学基础与符号约定

#

下面的内容不需要在事前理解，只需要在遇到新内容的时候查询即可。

0. 加粗的小写字母表示向量（通常为列向量），如 $\mathbf{x}_{k}$、$\mathbf{u}_{k}$。
1. 加粗的大写字母表示矩阵，如 $\mathbf{F}$、$\mathbf{B}$、$\mathbf{H}$、$\mathbf{Q}$、$\mathbf{R}$。
2. 头顶为^的字母表示估计值（后验估计），如 $\hat{\mathbf{x}}_{k}$。
3. 右上角为-的字母表示预测值（先验估计），如 $\mathbf{x}_{k}^{-}$。
4. 期望（表示数据分布的中心位置）：对于离散随机向量 $\mathbf{X}$，其期望为 $E[\mathbf{X}] = \sum_{i=1}^{n} \mathbf{X}_{i} P(\mathbf{X}_{i})$（连续情形为积分形式）。
5. 协方差矩阵（表示数据分布的不确定性和形状，在卡尔曼滤波中度量后验估计值的精确程度）：对于随机向量 $\mathbf{X}$，其协方差矩阵为 $Cov(\mathbf{X}) = E[(\mathbf{X} - E[\mathbf{X}])(\mathbf{X} - E[\mathbf{X}])^{T}]$。
6. 先验：指在获得新证据之前，对未知事件的预先判断、信念或概率。
7. 后验：指在获得新证据之后，将新证据与先验判断结合，得出的更新后的判断、信念或概率。
8. 转置：对于矩阵 $\mathbf{A}$，其转置为 $\mathbf{A}^{T}$。
9. 单位矩阵：$\mathbf{I}$，是方阵，且对角线上的元素为 1，其余元素为 0。
10. 迹：对于矩阵 $\mathbf{A}$，其迹为 $tr(\mathbf{A}) = \sum_{i=1}^{n} \mathbf{A}_{ii}$。
11. 逆：对于方阵 $\mathbf{A}$，其逆矩阵记为 $\mathbf{A}^{-1}$，满足 $\mathbf{A}\mathbf{A}^{-1} = \mathbf{A}^{-1}\mathbf{A} = \mathbf{I}$。

基本模型

#

（线性）卡尔曼滤波的应用基于以下三个假设前提：

1. 马尔可夫性：当前时刻状态只和上一时刻状态有关。
2. 线性模型：系统的状态转移和观测过程均满足线性关系。
3. 高斯噪声：过程噪声和测量噪声都符合高斯分布。

基于上述假设，我们可以得到如下两个公式：

$$ 过程模型： \mathbf{x}_{k} = \mathbf{F}_{k} \mathbf{x}_{k-1} + \mathbf{B}_{k} \mathbf{u}_{k} + \mathbf{w}_{k-1} \quad\quad ① $$$$ 观测模型： \mathbf{z}_{k} = \mathbf{H}_{k} \mathbf{x}_{k} + \mathbf{v}_{k} \quad\quad ② $$

其中：

• $\mathbf{x}_{k}$ 表示 $k$ 时刻的真实状态值，是待估计的未知量；
• $\mathbf{x}_{k-1}$ 表示 $k-1$ 时刻的真实状态值；
• $\mathbf{u}_{k}$ 表示 $k$ 时刻的控制输入量（作用于区间 $(k-1, k]$），描述在 $k-1$ 时刻至 $k$ 时刻这段时间里生效的输入；
• $\mathbf{w}_{k-1}$ 表示过程噪声，其均值为 $0$，协方差矩阵为 $\mathbf{Q}_{k-1}$，即 $\mathbf{w}_{k-1} \sim N(0, \mathbf{Q}_{k-1})$；
• $\mathbf{z}_{k}$ 表示 $k$ 时刻的观测值，是已知的测量量；
• $\mathbf{v}_{k}$ 表示观测噪声，其均值为 $0$，协方差矩阵为 $\mathbf{R}_{k}$，即 $\mathbf{v}_{k} \sim N(0, \mathbf{R}_{k})$；
• $\mathbf{F}_{k}$ 表示状态转移矩阵，描述状态如何从 $k-1$ 时刻演化到 $k$ 时刻；
• $\mathbf{B}_{k}$ 表示控制矩阵，描述区间 $(k-1, k]$ 内生效的控制输入 $\mathbf{u}_{k}$ 如何影响 $k$ 时刻的状态；
• $\mathbf{H}_{k}$ 表示观测矩阵，描述系统的 $k$ 时刻真实状态 $\mathbf{x}_{k}$ 与我们得到的测量值 $\mathbf{z}_{k}$ 之间的关系；

然而现实中，我们无法得到真实的噪声 $\mathbf{w}_{k-1}$ 和 $\mathbf{v}_{k}$ ，因此即便我们建立了精确的模型，也无法得到准确的真实状态值 $\mathbf{x}_{k}$。所以我们希望找到一个最优估计值 $\hat{\mathbf{x}}_{k}$ 来尽可能地逼近 $\mathbf{x}_{k}$，使得估计误差最小。

预测步骤：推导先验估计值

#

根据过程模型 ① $\mathbf{x}_{k} = \mathbf{F}_{k} \mathbf{x}_{k-1} + \mathbf{B}_{k} \mathbf{u}_{k} + \mathbf{w}_{k-1}$，我们可以对 $k$ 时刻的状态进行预测。

由于我们无法得知上一时刻的真实状态 $\mathbf{x}_{k-1}$ 和当前的过程噪声 $\mathbf{w}_{k-1}$，因此我们使用上一时刻的最优估计值 $\hat{\mathbf{x}}_{k-1}$ 来代替 $\mathbf{x}_{k-1}$。同时，由于过程噪声 $\mathbf{w}_{k-1}$ 的期望为零，在预测时我们取其期望值，即 $E[\mathbf{w}_{k-1}]=0$。

由此，我们得到 $k$ 时刻状态的先验估计值（预测值） $\mathbf{x}_{k}^{-}$ 的计算公式：

$$ \mathbf{x}_{k}^{-} = \mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} + \mathbf{B}_{k} \mathbf{u}_{k} $$

其中：

• $\mathbf{x}_{k}^{-}$ 表示 $k$ 时刻状态的预测值，也叫先验估计值（因为它是在获得 $k$ 时刻测量值之前计算的）；
• $\hat{\mathbf{x}}_{k-1}$ 表示 $k-1$ 时刻的最优估计值（后验估计值）；
• $\mathbf{u}_{k}$ 表示 $k$ 时刻的控制输入量（作用于区间 $(k-1, k]$）；
• $\mathbf{F}_{k}$ 表示状态转移矩阵，描述真实状态 $\mathbf{x}$ 如何从 $k-1$ 时刻演化到 $k$ 时刻；
• $\mathbf{B}_{k}$ 表示控制矩阵，描述在区间 $(k-1, k]$ 生效的控制输入 $\mathbf{u}_{k}$ 如何影响 $k$ 时刻的状态 $\mathbf{x}$；

这个预测值 $\mathbf{x}_{k}^{-}$ 仅仅基于系统的动态模型和上一时刻的状态估计，它没有包含当前 $k$ 时刻的任何测量信息。因此，它是一个有待修正的初步估计。下一步，我们将利用 $k$ 时刻的测量值 $\mathbf{z}_{k}$ 来修正这个预测值，以获得更精确的后验估计值 $\hat{\mathbf{x}}_{k}$。

更新步骤：融合测量值修正预测

#

在预测步骤中，我们得到了一个 $k$ 时刻的先验预测值 $\mathbf{x}_{k}^{-}$。这个值仅基于系统模型和上一时刻的状态，尚未利用 $k$ 时刻的测量数据。现在，我们的任务是利用 $k$ 时刻的实际测量值 $\mathbf{z}_{k}$ 来修正这个预测，从而得到一个更准确的后验估计值 $\hat{\mathbf{x}}_{k}$。

修正的核心思路是比较“实际测量”与“预测的测量”之间的差距。

1. $k$ 时刻的实际测量值：$\mathbf{z}_{k}$
2. $k$ 时刻的预测测量值：$\mathbf{H}_{k}\mathbf{x}_{k}^{-}$（这是将状态预测值 $\mathbf{x}_{k}^{-}$ 通过观测模型 $\mathbf{H}_{k}$ 映射到测量空间得到的值）

$k$ 时刻的实际测量值 $\mathbf{z}_{k}$ 与 $k$ 时刻的预测测量值 $\mathbf{H}_{k}\mathbf{x}_{k}^{-}$ 之差，被称为测量残差（Innovation） $Y_{k}$：

$$ Y_{k} = \mathbf{z}_{k} - \mathbf{H}_{k}\mathbf{x}_{k}^{-} $$

这个测量残差 $Y_{k}$ 反映了我们的预测与现实的差距。如果残差很小，说明预测准确；如果残差很大，则说明预测存在较大偏差，需要进行显著修正。

卡尔曼滤波通过一个核心的状态更新方程来完成修正，该方程将最终的后验估计值表示为先验预测值与加权后的测量残差之和：

$$ 状态更新方程： \hat{\mathbf{x}}_{k} = \mathbf{x}_{k}^{-} + \mathbf{K}_{k}(\mathbf{z}_{k} - \mathbf{H}_{k}\mathbf{x}_{k}^{-}) \quad\quad ③ $$

其中：

• $\hat{\mathbf{x}}_{k}$ 是 $k$ 时刻的最优估计值（或称后验估计值），因为它融合了 $k$ 时刻的测量信息。
• $\mathbf{K}_{k}$ 是卡尔曼增益。它的核心作用是一个权重矩阵，用于权衡我们应该在多大程度上相信“预测值”和“测量值”。
  • 如果测量噪声很大（即我们不信任测量值），$\mathbf{K}_{k}$ 的值会变小，我们就更倾向于相信预测值 $\mathbf{x}_{k}^{-}$。
  • 如果预测本身很不确定（即我们不信任预测值），$\mathbf{K}_{k}$ 的值会变大，我们就更倾向于利用测量信息进行大幅修正。

至此，状态更新的逻辑已经建立。即从 $k-1$ 时刻的后验估计值 $\hat{\mathbf{x}}_{k-1}$ 经过 $k$ 时刻状态的先验估计值（预测值） $\mathbf{x}_{k}^{-}$ 到 $k$ 时刻的最优估计值 $\hat{\mathbf{x}}_{k}$ 的更新过程已经完成。

但卡尔曼增益 $\mathbf{K}_{k}$ 还是未知的，后续的推导将致力于如何计算这个使估计误差最小的最优卡尔曼增益 $\mathbf{K}_{k}$。

目标函数的建立与转化

#

将上述思路转化为数学语言，我们首先定义后验估计误差 $\mathbf{e}_{k} = \mathbf{x}_{k} - \hat{\mathbf{x}}_{k}$，即 $k$ 时刻真实状态值与后验估计值的差。卡尔曼滤波的目标是寻找一个最优的增益矩阵 $\mathbf{K}_k$ 来最小化该误差的均方值 $E[||\mathbf{e}_k||^2]$。

这个均方误差可以通过后验误差协方差矩阵 $\mathbf{P}_{k}$ 的迹来表示，其中 $\mathbf{P}_{k}$ 定义为：

$$ \mathbf{P}_{k} = E[\mathbf{e}_{k}\mathbf{e}_{k}^{T}] $$

它们之间的关系如下：

$$ E[||\mathbf{e}_k||^2] = E[\mathbf{e}_k^T \mathbf{e}_k] = \text{tr}(E[\mathbf{e}_k \mathbf{e}_k^T]) = \text{tr}(\mathbf{P}_k) $$

因此，优化目标函数可以明确地表示为：

$$ \min_{\mathbf{K}_k}{\text{tr}(\mathbf{P}_{k})} $$

为了求解这个最小化问题，我们需要推导出 $\mathbf{P}_{k}$ 关于 $\mathbf{K}_{k}$ 的具体表达式。这需要我们先从 $\mathbf{e}_{k}$ 的表达式入手。根据状态更新方程 ③ $\hat{\mathbf{x}}_{k} = \mathbf{x}_{k}^{-} + \mathbf{K}_{k}(\mathbf{z}_{k} - \mathbf{H}_{k}\mathbf{x}_{k}^{-})$ 和观测模型 ② $\mathbf{z}_{k} = \mathbf{H}_{k} \mathbf{x}_{k} + \mathbf{v}_{k}$，我们有：

$$ \begin{aligned} \mathbf{e}_{k} &= \mathbf{x}_{k} - \hat{\mathbf{x}}_{k} \\ &= \mathbf{x}_{k} - \left( \mathbf{x}_{k}^{-} + \mathbf{K}_{k}(\mathbf{z}_{k} - \mathbf{H}_{k}\mathbf{x}_{k}^{-}) \right) \\ &= (\mathbf{x}_{k} - \mathbf{x}_{k}^{-}) - \mathbf{K}_{k}(\mathbf{H}_{k}\mathbf{x}_{k} + \mathbf{v}_{k}-\mathbf{H}_{k}\mathbf{x}_{k}^{-} ) \\ &= (\mathbf{x}_{k} - \mathbf{x}_{k}^{-}) - \mathbf{K}_{k}\mathbf{H}_{k}(\mathbf{x}_{k} - \mathbf{x}_{k}^{-}) - \mathbf{K}_{k}\mathbf{v}_{k} \\ &= (\mathbf{I} - \mathbf{K}_{k}\mathbf{H}_{k})(\mathbf{x}_{k} - \mathbf{x}_{k}^{-} ) - \mathbf{K}_{k}\mathbf{v}_{k} \end{aligned} $$

为简化表示，我们引入先验估计误差 $\mathbf{e}_{k}^{-} = \mathbf{x}_{k} - \mathbf{x}_{k}^{-}$（$k$ 时刻真实状态值与先验估计值的差），则上式变为：

$$ \mathbf{e}_{k} = (\mathbf{I} - \mathbf{K}_{k}\mathbf{H}_{k})\mathbf{e}_{k}^{-} - \mathbf{K}_{k}\mathbf{v}_{k} \quad\quad ④ $$

现在，我们可以利用公式 ④ 来计算后验误差协方差矩阵 $\mathbf{P}_{k}$。在这个过程中，我们还需要用到先验误差协方差矩阵 $\mathbf{P}_{k}^{-}$，其定义为：

$$ \mathbf{P}_{k}^{-} = E[\mathbf{e}_{k}^{-}\mathbf{e}_{k}^{-T}] $$

根据协方差矩阵的定义，$\mathbf{P}_{k}$ 和 $\mathbf{P}_{k}^{-}$ 都是对称矩阵。对公式 ④ 两边乘以其自身的转置，并取期望，可得：

$$ \begin{aligned} \mathbf{P}_{k} &= E[\mathbf{e}_{k} \mathbf{e}_{k}^{T}] \\ &= E\!\left[ \big((\mathbf{I} - \mathbf{K}_{k}\mathbf{H}_{k})\mathbf{e}_{k}^{-} - \mathbf{K}_{k}\mathbf{v}_{k}\big) \big((\mathbf{I} - \mathbf{K}_{k}\mathbf{H}_{k})\mathbf{e}_{k}^{-} - \mathbf{K}_{k}\mathbf{v}_{k}\big)^{T} \right] \\ &= (\mathbf{I} - \mathbf{K}_{k}\mathbf{H}_{k})\mathbf{P}_{k}^{-}(\mathbf{I} - \mathbf{K}_{k}\mathbf{H}_{k})^{T} + \mathbf{K}_{k}\mathbf{R}_{k}\mathbf{K}_{k}^{T} \\ &= \mathbf{P}_{k}^{-} - \mathbf{K}_{k}\mathbf{H}_{k}\mathbf{P}_{k}^{-} - \mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}\mathbf{K}_{k}^{T} + \mathbf{K}_{k}(\mathbf{H}_{k}\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}+\mathbf{R}_{k})\mathbf{K}_{k}^{T} \quad\quad ⑤ \end{aligned} $$

其中，$\mathbf{R}_{k}$ 是测量噪声协方差。

至此，我们将随机变量的最优化问题转化成为了关于矩阵 $\mathbf{K}_k$ 的函数最小化问题。很多文章在此直接对后验误差协方差矩阵 $\mathbf{P}_{k}$ 取迹，然后对卡尔曼增益 $\mathbf{K}_k$ 求导，并令其为零，以获得使 $\text{tr}(\mathbf{P}_{k})$ 最小的 $\mathbf{K}_k$ 值。如此做的理由有以下三点：

1. $\text{tr}(\mathbf{P}_{k})$ 在物理上表示系统状态估计误差的总方差，即均方误差 $E[||\mathbf{e}_k||^2]$。最小化迹就是最小化整体的估计误差能量，这是一个非常直观且合理的优化目标。
2. $\text{tr}(\mathbf{P}_{k})$ 是关于 $\mathbf{K}_k$ 的一个标量函数，并且可以证明它是一个二次函数，因此是凸函数，存在唯一的全局最小值。
3. 对标量函数 $\text{tr}(\mathbf{P}_{k})$ 进行矩阵求导（$\frac{\partial \text{tr}(\mathbf{P}_{k})}{\partial \mathbf{K}_k}$）比直接对矩阵 $\mathbf{P}_{k}$ 求导要简单得多，并且有成熟的矩阵迹求导法则（如 $\frac{\partial \text{tr}(AB)}{\partial A} = B^T$）可以使用。

卡尔曼增益求解和协方差矩阵化简

#

根据上述扩展知识，我们求解最优卡尔曼增益 $\mathbf{K}_{k}$，

$$ \begin{align} 令 \frac{\partial \text{tr}(\mathbf{P}_{k})}{\partial \mathbf{K}_{k}} &= \frac{\partial \text{tr}(\mathbf{P}_{k}^{-}) }{\partial \mathbf{K}_{k}} -\frac{\partial \text{tr}(\mathbf{K}_{k}\mathbf{H}_{k}\mathbf{P}_{k}^{-})}{\partial \mathbf{K}_{k}}-\frac{\partial \text{tr}(\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}\mathbf{K}_{k}^{T})}{\partial \mathbf{K}_{k}}+\frac{\partial \text{tr}(\mathbf{K}_{k}(\mathbf{H}_{k}\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}+\mathbf{R}_{k})\mathbf{K}_{k}^{T})}{\partial \mathbf{K}_{k}} \\ &= 0 - (\mathbf{H}_{k}\mathbf{P}_{k}^{-})^{T} - (\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}) + 2\mathbf{K}_{k}(\mathbf{H}_{k}\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}+\mathbf{R}_{k}) \\ &= -2\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T} + 2\mathbf{K}_{k}(\mathbf{H}_{k}\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}+\mathbf{R}_{k}) \\ &= 0 \end{align} $$

我们得到

$$ \mathbf{K}_{k} = \mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}(\mathbf{H}_{k}\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}+\mathbf{R}_{k})^{-1} \quad\quad ⑥ $$

其中， $\mathbf{R}_{k}$ 是测量噪声协方差。

代入到后验误差协方差公式 $\mathbf{P}_{k} = (\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k})\mathbf{P}_{k}^{-}(\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k})^T + \mathbf{K}_{k}\mathbf{R}_{k}\mathbf{K}_{k}^T$ 中，展开后可得：$\mathbf{P}_{k} = \mathbf{P}_{k}^{-} - \mathbf{K}_{k}\mathbf{H}_{k}\mathbf{P}_{k}^{-} - \mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}\mathbf{K}_{k}^{T} + \mathbf{K}_{k}(\mathbf{H}_{k}\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}+\mathbf{R}_{k})\mathbf{K}_{k}^{T}$。

利用公式 ⑥ 的结论 $\mathbf{K}_{k}(\mathbf{H}_{k}\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}+\mathbf{R}_{k}) = \mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}$，对上式进行化简，则有后验误差协方差矩阵：

$$ \begin{align} \mathbf{P}_{k} &= \mathbf{P}_{k}^{-} - \mathbf{K}_{k}\mathbf{H}_{k}\mathbf{P}_{k}^{-} - \mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}\mathbf{K}_{k}^{T} + (\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T})\mathbf{K}_{k}^{T} \\ &= \mathbf{P}_{k}^{-} - \mathbf{K}_{k}\mathbf{H}_{k}\mathbf{P}_{k}^{-} \\ &= (\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k})\mathbf{P}_{k}^{-} \quad\quad ⑦ \end{align} $$

借鉴上述整个推导过程，对于由运动模型得到的 $k$ 时刻状态的先验估计值（预测值）公式 $\mathbf{x}_{k}^{-} = \mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} + \mathbf{B}_{k}\mathbf{u}_{k}$。

为了构建先验误差 $\mathbf{e}_{k}^{-}$，按定义两式相减：

$$ \begin{align} \mathbf{e}_{k}^{-} &= \mathbf{x}_{k}-\mathbf{x}_{k}^{-} \\ &= \mathbf{x}_{k}-\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} - \mathbf{B}_{k}\mathbf{u}_{k} \\ &= \mathbf{F}_{k} \mathbf{x}_{k-1} + \mathbf{B}_{k}\mathbf{u}_{k} +\mathbf{w}_{k-1}-\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} - \mathbf{B}_{k}\mathbf{u}_{k} \\ &= \mathbf{F}_{k}( \mathbf{x}_{k-1}-\hat{\mathbf{x}}_{k-1})+\mathbf{w}_{k-1} \end{align} $$

即

$$ \mathbf{e}_{k}^{-} = \mathbf{F}_{k}\mathbf{e}_{k-1}+\mathbf{w}_{k-1} $$

类似构建后验误差协方差矩阵 $\mathbf{P}_{k}$ 的方式，同样构建先验误差协方差矩阵 $\mathbf{P}_{k}^{-}$，两边同时乘以自身的转置并取期望，再考虑到 $\mathbf{w}_{k-1}$ 和 $\mathbf{e}_{k-1}$ 互相独立（即 $E[\mathbf{F}_{k}\mathbf{e}_{k-1}\mathbf{w}_{k-1}^T]=0$），有

$$ \begin{aligned} E[\mathbf{e}_{k}^{-}\mathbf{e}_{k}^{-T}] &= E\!\left[(\mathbf{F}_{k}\mathbf{e}_{k-1}+\mathbf{w}_{k-1})(\mathbf{F}_{k}\mathbf{e}_{k-1}+\mathbf{w}_{k-1})^{T}\right] \\ &= \mathbf{F}_{k}\,E[\mathbf{e}_{k-1}\mathbf{e}_{k-1}^{T}]\,\mathbf{F}_{k}^{T}+E[\mathbf{w}_{k-1}\mathbf{w}_{k-1}^{T}] \end{aligned} $$

则有先验误差协方差矩阵：

$$ \mathbf{P}_{k}^{-}= \mathbf{F}_{k}\mathbf{P}_{k-1}\mathbf{F}_{k}^{T}+\mathbf{Q}_{k-1} \quad\quad ⑧ $$

其中， $\mathbf{Q}_{k-1}$ 为过程噪声协方差。

至此，我们已经获得了完整的卡尔曼滤波预测、更新的公式。

梳理

#

我们对上文做一个系统性的总结。

首先，我们对实际问题进行建模，获得运动模型和观测模型：

运动模型： $\mathbf{x}_{k} = \mathbf{F}_{k} \mathbf{x}_{k-1} + \mathbf{B}_{k}\mathbf{u}_{k} +\mathbf{w}_{k-1} \quad\quad ①$

观测模型： $\mathbf{z}_{k} = \mathbf{H}_{k} \mathbf{x}_{k} +\mathbf{v}_{k} \quad\quad ②$

其中，$\mathbf{w}_{k-1}$ 和 $\mathbf{v}_{k}$ 分别是过程噪声和观测噪声，它们是互不相关的高斯白噪声，其协方差矩阵分别为 $\mathbf{Q}_{k-1}$ 和 $\mathbf{R}_{k}$。

其次，我们通过无偏估计的假设和误差定义，获得最优估计值和协方差矩阵的表达式（更新步骤）：

状态更新： $\hat{\mathbf{x}}_{k} = \mathbf{x}_{k}^{-} + \mathbf{K}_{k}(\mathbf{z}_{k}-\mathbf{H}_{k}\mathbf{x}_{k}^{-} ) \quad\quad ③$

后验误差： $\mathbf{e}_{k} = (\mathbf{I} -\mathbf{K}_{k}\mathbf{H}_{k})\mathbf{e}_{k}^{-} - \mathbf{K}_{k}\mathbf{v}_{k} \quad\quad ④$

后验误差协方差矩阵（展开形式）： $\mathbf{P}_{k} = E[\mathbf{e}_{k}\mathbf{e}_{k}^{T}]= \mathbf{P}_{k}^{-} - \mathbf{K}_{k}\mathbf{H}_{k}\mathbf{P}_{k}^{-}-\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}\mathbf{K}_{k}^{T} + \mathbf{K}_{k}(\mathbf{H}_{k}\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}+\mathbf{R}_{k})\mathbf{K}_{k}^{T} \quad\quad ⑤$

再次，我们通过最小化后验误差协方差矩阵 $\mathbf{P}_{k}$ 的迹，推导出卡尔曼增益：

卡尔曼增益： $\mathbf{K}_{k}=\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}(\mathbf{H}_{k}\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}+\mathbf{R}_{k})^{-1} \quad\quad ⑥$

将上式代入 ⑤ 并化简，可得后验误差协方差矩阵的更简洁形式：

后验误差协方差矩阵（常用形式）： $\mathbf{P}_{k} = (\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k})\mathbf{P}_{k}^{-} \quad\quad ⑦$

最后，我们进行预测步骤，计算下一时刻的先验量：

先验误差协方差矩阵： $\mathbf{P}_{k}^{-}= \mathbf{F}_{k}\mathbf{P}_{k-1}\mathbf{F}_{k}^{T}+\mathbf{Q}_{k-1} \quad\quad ⑧$

状态预测： $\mathbf{x}_{k}^{-} = \mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} + \mathbf{B}_{k}\mathbf{u}_{k} \quad\quad ⑨$

先验误差： $\mathbf{e}_{k}^{-} = \mathbf{F}_{k}\mathbf{e}_{k-1}+\mathbf{w}_{k-1}$

总结

#

那么，卡尔曼滤波就可以按下面的 5 个公式理解并说明：

实现过程：使用上一次的最优结果预测出当前值，同时使用当前观测值修正，得到当前最优结果。

我们将卡尔曼滤波的完整流程总结为两个阶段：预测和更新。

• 预测

  1. 预测状态 $$ \mathbf{x}_{k}^{-} = \mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} + \mathbf{B}_{k} \mathbf{u}_{k} \quad\quad (1) $$
  2. 预测先验误差协方差矩阵 $$ \mathbf{P}_{k}^{-} = \mathbf{F}_{k}\mathbf{P}_{k-1}\mathbf{F}_{k}^{T} + \mathbf{Q}_{k-1} \quad\quad (2) $$

• 更新

  1. 计算卡尔曼增益 $$ \mathbf{K}_{k} = \mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}(\mathbf{H}_{k}\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{T}+\mathbf{R}_{k})^{-1} \quad\quad (3) $$
  2. 更新状态估计 $$ \hat{\mathbf{x}}_{k} = \mathbf{x}_{k}^{-} + \mathbf{K}_{k}(\mathbf{z}_{k} - \mathbf{H}_{k}\mathbf{x}_{k}^{-}) \quad\quad (4) $$
  3. 更新后验误差协方差矩阵 $$ \mathbf{P}_{k} = (\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k})\mathbf{P}_{k}^{-} \quad\quad (5) $$

在设定好系统的模型参数（状态转移矩阵：$\mathbf{F}_k$、控制矩阵：$\mathbf{B}_k$、观测矩阵：$\mathbf{H}_k$）和噪声协方差（过程噪声协方差：$\mathbf{Q}_k$，测量噪声协方差：$\mathbf{R}_k$）后，这个预测-更新的循环从初始状态 $\hat{\mathbf{x}}_0$ 和初始误差协方差矩阵 $\mathbf{P}_0$ 开始，不断迭代，实现对系统状态的实时最优估计。

实践

#

调节超参数

#

1. 过程噪声协方差：$\mathbf{Q}_k$ 和测量噪声协方差：$\mathbf{R}_k$

2. 初始状态 $\hat{\mathbf{x}}_0$ 和初始误差协方差矩阵 $\mathbf{P}_0$

卡尔曼滤波的使用步骤

#

1. 选择状态量、观测量
2. 构建方程
3. 初始化参数（$\mathbf{Q}_k$、$\mathbf{R}_k$、$\hat{\mathbf{x}}_0$（一般取 0）、$\mathbf{P}_0$（一般取 1，不可为 0））
4. 代入公式迭代
5. 调节超参数（$\mathbf{Q}_k$、$\mathbf{R}_k$）

案例分析一：一维时序信号的追踪与滤波

#

本节将以一个常见的工程问题——从带噪声的观测中恢复纯净的正弦信号——为例，详细演示如何为特定问题构建卡尔曼滤波模型。核心挑战在于，正弦波本身并非一个简单的动态系统，我们需要通过数学推导，将其转换为卡尔曼滤波框架所要求的线性状态空间形式。

第一步：建立信号的动态模型

#

卡尔曼滤波要求系统满足马尔可夫性，即当前状态仅与上一状态有关。幸运的是，一个纯净的、离散采样的正弦波，其未来值 $y_{t+1}$ 可以由它当前和过去的值 $y_t$, $y_{t-1}$ 精确地线性表示。我们的首要任务就是找到这个线性递推关系。

我们从离散正弦波的定义出发：

$$ y_t = A \sin(\omega t + \phi) $$

其中 $\omega$ 是数字角频率（ $\omega = 2\pi f / f_s$ ，f 是信号频率， $f_s$ 是采样频率）。

我们写出 $t+1$ 和 $t-1$ 时刻的表达式：

1. $y_{t+1} = A \sin(\omega(t+1) + \phi) = A \sin((\omega t+\phi) + \omega)$
2. $y_{t-1} = A \sin(\omega(t-1) + \phi) = A \sin((\omega t+\phi) - \omega)$

使用三角函数的和角公式 $\sin(\alpha \pm \beta) = \sin(\alpha)\cos(\beta) \pm \cos(\alpha)\sin(\beta)$ 展开上述两式：

1. $y_{t+1} = A [\sin(\omega t+\phi)\cos(\omega) + \cos(\omega t+\phi)\sin(\omega)]$
2. $y_{t-1} = A [\sin(\omega t+\phi)\cos(\omega) - \cos(\omega t+\phi)\sin(\omega)]$

将这两个式子相加，$\cos(\omega t+\phi)\sin(\omega)$ 项会正负抵消：

$$ y_{t+1} + y_{t-1} = 2 A \sin(\omega t+\phi) \cos(\omega) $$

注意到 $A \sin(\omega t+\phi)$ 正好就是 $y_t$，将其代入上式：

$$ y_{t+1} + y_{t-1} = 2 y_t \cos(\omega) $$

整理得到 $y_{t+1}$ 的表达式，这便是我们需要的线性递推关系：

$$ y_{t+1} = 2\cos(\omega) y_t - y_{t-1} $$

这个公式是建模的关键。它揭示了正弦信号内在的、二阶的线性动态特性，为我们构建状态转移模型铺平了道路。

第二步：构建状态空间方程

#

我们得到的递推关系是二阶的（依赖于 $y_t$ 和 $y_{t-1}$ ），而标准卡尔曼滤波的状态转移是一阶的（$\mathbf{x}_k = \mathbf{F}_k \mathbf{x}_{k-1} + \dots$）。解决这个问题的经典方法是“状态增广”：通过增加状态向量的维度，将高阶系统转化为一阶系统。

1. 定义状态向量 $\mathbf{x}_t$ 为实现一阶递推，状态向量必须包含计算 $y_{t+1}$ 所需的所有历史信息，即 $y_t$ 和 $y_{t-1}$。因此，我们定义 $t$ 时刻的状态向量为：

   $$ \mathbf{x}_t = \begin{bmatrix} y_t \\ y_{t-1} \end{bmatrix} $$

2. 构建状态转移矩阵 $\mathbf{F}$ 我们的目标是找到一个 2x2 矩阵 $\mathbf{F}$，使得 $\mathbf{x}_{t+1} = \mathbf{F} \mathbf{x}_t$ 成立。首先写出 $\mathbf{x}_{t+1}$ 的定义：

   $$ \mathbf{x}_{t+1} = \begin{bmatrix} y_{t+1} \\ y_t \end{bmatrix} $$

   现在，将状态方程展开：

   $$ \begin{bmatrix} y_{t+1} \\ y_t \end{bmatrix} = \begin{bmatrix} F_{11} & F_{12} \\ F_{21} & F_{22} \end{bmatrix} \begin{bmatrix} y_t \\ y_{t-1} \end{bmatrix} $$

   我们逐行确定 $\mathbf{F}$ 的元素：

   • 第一行：$y_{t+1} = F_{11} y_t + F_{12} y_{t-1}$ 根据第一步的递推关系 $y_{t+1} = (2\cos(\omega)) y_t + (-1) y_{t-1}$，可得： $F_{11} = 2\cos(\omega)$ 和 $F_{12} = -1$
   • 第二行：$y_t = F_{21} y_t + F_{22} y_{t-1}$ 这是一个恒等关系：$t+1$ 时刻状态的第二个元素（$y_t$）就是 $t$ 时刻状态的第一个元素。所以： $y_t = (1) y_t + (0) y_{t-1}$ 可得：$F_{21} = 1$ 和 $F_{22} = 0$

   组合起来，我们就得到了状态转移矩阵 $\mathbf{F}$：

   $$ \mathbf{F} = \begin{bmatrix} 2\cos(\omega) & -1 \\ 1 & 0 \end{bmatrix} $$

第三步：定义观测模型

#

模型建立的最后一步是关联我们定义的内部状态与外部的实际测量值。

1. 定义观测量 $\mathbf{z}_t$ 在我们的问题中，每次的测量值 $\mathbf{z}_t$ 就是带有噪声的、当前时刻的正弦波幅值。

2. 构建观测矩阵 $\mathbf{H}$ 我们需要一个矩阵 $\mathbf{H}$，它能从状态向量 $\mathbf{x}_t = \begin{bmatrix} y_t \\ y_{t-1} \end{bmatrix}$ 中“提取”出我们理论上要测量的值 $y_t$。

   $$ \mathbf{z}_t \approx \mathbf{H} \mathbf{x}\_t = \begin{bmatrix} H_1 & H_2 \end{bmatrix} \begin{bmatrix} y_t \\ y_{t-1} \end{bmatrix} = H_1 y_t + H_2 y_{t-1} $$

   为了让 $\mathbf{H} \mathbf{x}_t$ 等于 $y_t$，我们只需令 $H_1 = 1$ 和 $H_2 = 0$。因此，观测矩阵 $\mathbf{H}$ 是：

   $$ \mathbf{H} = \begin{bmatrix} 1 & 0 \end{bmatrix} $$

模型总结

#

至此，我们成功地将一个看似复杂的正弦波信号，转化为了标准的线性高斯系统模型，可以完美地套用卡尔曼滤波的五个核心公式。

• 过程模型：$\mathbf{x}_{k} = \mathbf{F} \mathbf{x}_{k-1} + \mathbf{w}_{k-1}$

  • 状态向量 $\mathbf{x}_{k} = \begin{bmatrix} y_k \\ y_{k-1} \end{bmatrix}$，代表信号当前和上一时刻的真实值。
  • 状态转移矩阵 $\mathbf{F} = \begin{bmatrix} 2\cos(\omega) & -1 \\ 1 & 0 \end{bmatrix}$，描述了正弦波的内在演化规律。
  • 过程噪声 $\mathbf{w}_{k-1}$ 代表了模型的不确定性（例如频率的微小漂移），其协方差为 $\mathbf{Q}$。

• 观测模型：$\mathbf{z}_{k} = \mathbf{H} \mathbf{x}_{k} + \mathbf{v}_{k}$

  • 观测矩阵 $\mathbf{H} = \begin{bmatrix} 1 & 0 \end{bmatrix}$，表示我们只能直接测量到信号的当前值。
  • 观测噪声 $\mathbf{v}_{k}$ 代表了传感器的测量误差，其协方差为 $\mathbf{R}$。

在实际应用中，我们只需根据信号的频率确定 $\omega$ 并设置好 $\mathbf{F}$ 矩阵，再根据经验调节噪声协方差 $\mathbf{Q}$ 和 $\mathbf{R}$，就可以实现对带噪正弦波的高效滤波。

案例分析二：二维平面运动轨迹的估计与预测

#

在许多实际应用中，如雷达跟踪、视频监控或机器人导航，我们都需要根据一系列不精确的观测点来估计目标的真实运动轨迹。本节将以一个在二维平面上做近似匀加速运动的目标为例，展示如何构建卡尔曼滤波器来实时估计其精确的位置和速度，并对未来位置进行预测。

第一步：建立目标的动态模型

#

我们假设目标在每个采样时间间隔 $\Delta t$ 内，其加速度是近似恒定的，但会受到一些随机扰动（例如，轻微的机动、空气阻力变化等）。基于牛顿运动学定律，我们可以建立目标的状态模型。

1. 定义状态向量 $\mathbf{x}_k$ 为了完整描述目标在二维平面上的运动状态，我们需要知道它的位置和速度。因此，状态向量应包含 $x$ 和 $y$ 两个方向上的位置和速度分量：

   $$ \mathbf{x}_k = \begin{bmatrix} p_{x,k} \\ p_{y,k} \\ v_{x,k} \\ v_{y,k} \end{bmatrix} $$

   其中，$p$ 代表位置 (position)，$v$ 代表速度 (velocity)。

2. 构建状态转移矩阵 $\mathbf{F}$ 状态转移矩阵 $\mathbf{F}$ 描述了在没有噪声的情况下，状态如何从 $k-1$ 时刻演化到 $k$ 时刻。根据匀加速运动公式：

   • $p_k = p_{k-1} + v_{k-1}\Delta t + \frac{1}{2}a_{k-1}\Delta t^2$
   • $v_k = v_{k-1} + a_{k-1}\Delta t$

   为简化模型，我们通常将加速度作为过程噪声的一部分来处理，而不是直接放入状态向量。这被称为恒速模型 (Constant Velocity, CV)，它假设在每个 $\Delta t$ 内速度恒定。这个模型足以应对大多数机动性不强的目标。

   • $p_{x,k} = p_{x,k-1} + v_{x,k-1}\Delta t$
   • $p_{y,k} = p_{y,k-1} + v_{y,k-1}\Delta t$
   • $v_{x,k} = v_{x,k-1}$
   • $v_{y,k} = v_{y,k-1}$

   将上述关系写成矩阵形式 $\mathbf{x}_k = \mathbf{F} \mathbf{x}_{k-1}$：

   $$ \begin{bmatrix} p_{x,k} \\ p_{y,k} \\ v_{x,k} \\ v_{y,k} \end{bmatrix} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} p_{x,k-1} \\ p_{y,k-1} \\ v_{x,k-1} \\ v_{y,k-1} \end{bmatrix} $$

   因此，状态转移矩阵为：

   $$ \mathbf{F} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} $$

3. 定义过程噪声协方差矩阵 $\mathbf{Q}$ 恒速模型是一个近似。目标的实际运动会因为未建模的加速度（随机扰动）而偏离该模型。过程噪声 $\mathbf{w}_{k-1}$ 正是用来描述这种不确定性。

   一个常用的方法是假设加速度是一个零均值、方差为 $\sigma_a^2$ 的随机过程。加速度在 $\Delta t$ 时间内对位置和速度的影响分别是 $\frac{1}{2}a\Delta t^2$ 和 $a\Delta t$。经过推导，可以得到一个适用于离散时间系统的过程噪声协方差矩阵 $\mathbf{Q}$：

   $$ \mathbf{Q} = \begin{bmatrix} \frac{\Delta t^4}{4} & 0 & \frac{\Delta t^3}{2} & 0 \\ 0 & \frac{\Delta t^4}{4} & 0 & \frac{\Delta t^3}{2} \\ \frac{\Delta t^3}{2} & 0 & \Delta t^2 & 0 \\ 0 & \frac{\Delta t^3}{2} & 0 & \Delta t^2 \end{bmatrix} \sigma_a^2 $$

   其中 $\sigma_a^2$ 是一个可调节的超参数，代表了我们对目标机动能力（即加速度变化剧烈程度）的预估。$\sigma_a^2$ 越大，滤波器对模型预测的信任度就越低，从而更依赖于测量值。

第二步：定义观测模型

#

接下来，我们需要建立内部状态与外部测量之间的关系。

1. 定义观测量 $\mathbf{z}_k$ 假设我们的传感器（如 GPS 或摄像头）只能直接测量目标的位置，而无法直接测量其速度。因此，观测向量为：

   $$ \mathbf{z}_k = \begin{bmatrix} p_{x, measured} \\ p_{y, measured} \end{bmatrix} $$

2. 构建观测矩阵 $\mathbf{H}$ 观测矩阵 $\mathbf{H}$ 的作用是从状态向量 $\mathbf{x}_k$ 中提取出与观测量相对应的部分。

   $$ \mathbf{z}_k \approx \mathbf{H} \mathbf{x}_k = \mathbf{H} \begin{bmatrix} p_{x,k} \\ p_{y,k} \\ v_{x,k} \\ v_{y,k} \end{bmatrix} $$

   为了提取出位置信息 $(p_{x,k}, p_{y,k})$，$\mathbf{H}$ 矩阵应为：

   $$ \mathbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{bmatrix} $$

3. 定义测量噪声协方差矩阵 $\mathbf{R}$ 任何传感器都存在测量误差。我们假设 $x$ 和 $y$ 方向的测量噪声是独立的，并且都服从高斯分布，其方差分别为 $\sigma_x^2$ 和 $\sigma_y^2$。这些值通常可以从传感器的规格手册中获得。

   $$ \mathbf{R} = \begin{bmatrix} \sigma_x^2 & 0 \\ 0 & \sigma_y^2 \end{bmatrix} $$

模型总结

#

通过以上步骤，我们为二维目标跟踪问题构建了完整的卡尔曼滤波模型：

• 过程模型：$\mathbf{x}_{k} = \mathbf{F} \mathbf{x}_{k-1} + \mathbf{w}_{k-1}$

  • 状态向量 $\mathbf{x}_{k}$ 包含了位置和速度信息。
  • 状态转移矩阵 $\mathbf{F}$ 基于恒速运动模型。
  • 过程噪声协方差 $\mathbf{Q}$ 反映了目标随机机动的可能性。

• 观测模型：$\mathbf{z}_{k} = \mathbf{H} \mathbf{x}_{k} + \mathbf{v}_{k}$

  • 观测矩阵 $\mathbf{H}$ 表明我们只能测量到位置。
  • 测量噪声协方差 $\mathbf{R}$ 代表了传感器的精度。

将这些矩阵代入卡尔曼滤波的五个核心公式，我们就可以通过迭代，从一系列带噪声的位置观测中，得到平滑的、包含速度信息的状态估计，从而实现对目标轨迹的精确跟踪和未来位置的短期预测。

案例分析三：三维空间中的刚体姿态跟踪问题

#

前面两个案例展示了卡尔曼滤波在线性系统中的强大威力。然而，当我们将目光投向更复杂的现实世界问题，如无人机飞行姿态、机器人手臂末端朝向或卫星在轨姿态的估计时，我们会遇到一个根本性的挑战： 旋转运动本质上是非线性的。

本节将探讨为什么三维姿态跟踪问题无法直接套用标准卡尔曼滤波，并简述解决此类问题的基本思路，以此展示标准 KF 的局限性，并引出卡尔曼滤波家族中的非线性分支。

第一步：姿态的数学表示

#

与位置可以用简单的向量 $(x, y, z)$ 表示不同，描述三维空间中的姿态（旋转）要复杂得多。常用的表示方法有：

1. 欧拉角（Euler Angles）：如滚转-俯仰-偏航角 $(\phi, \theta, \psi)$。这种表示直观，但在特定姿态（如俯仰角为 ±90°）时会出现万向节死锁（Gimbal Lock）问题，导致奇异性，无法正确表示旋转。
2. 旋转矩阵（Rotation Matrix）：一个 3x3 的正交矩阵 $\mathbf{C}$。它没有奇异性，但有 9 个参数，而姿态只有 3 个自由度，这带来了冗余和约束（$\mathbf{C}^T\mathbf{C} = \mathbf{I}, \det(\mathbf{C})=1$），计算复杂。
3. 四元数（Quaternion）：一个四维向量 $\mathbf{q} = [q_w, q_x, q_y, q_z]^T$，满足范数为 1 的约束（$||\mathbf{q}||=1$）。它既没有奇异性，也比旋转矩阵更紧凑，计算效率高，是姿态动力学中最常用的表示方法。

无论选择哪种表示，姿态的演化规律都无法用一个简单的线性方程 $\mathbf{x}_k = \mathbf{F}\mathbf{x}_{k-1}$ 来描述。

第二步：非线性的根源——姿态运动学

#

刚体的姿态变化率（角速度）与姿态本身的关系是非线性的。以四元数为例，其随时间变化的微分方程为：

$$ \dot{\mathbf{q}}(t) = \frac{1}{2} \mathbf{\Omega}(\boldsymbol{\omega}(t)) \mathbf{q}(t) $$

其中，$\mathbf{q}(t)$ 是姿态四元数，$\boldsymbol{\omega}(t) = [\omega_x, \omega_y, \omega_z]^T$ 是机体坐标系下的角速度向量，而 $\mathbf{\Omega}(\boldsymbol{\omega})$ 是一个与角速度相关的斜对称矩阵：

$$ \mathbf{\Omega}(\boldsymbol{\omega}) = \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix} $$

这是一个典型的非线性微分方程，因为状态的导数 $\dot{\mathbf{q}}$ 是状态 $\mathbf{q}$ 和输入 $\boldsymbol{\omega}$ 的乘积。将其离散化后，得到的状态转移方程 $\mathbf{q}_k = f(\mathbf{q}_{k-1}, \boldsymbol{\omega}_{k-1})$ 也是一个复杂的非线性函数，无法表示为 $\mathbf{F}\mathbf{q}_{k-1}$ 的形式。

第三步：非线性的观测模型

#

姿态的观测通常也呈非线性。例如，无人机通过加速度计和磁力计来确定姿态：

• 加速度计：在静止时测量重力向量 $\mathbf{g}$ 的方向。
• 磁力计：测量地球磁场向量 $\mathbf{m}$ 的方向。

当无人机姿态为 $\mathbf{q}$ 时，它在机体坐标系下观测到的重力向量 $\mathbf{a}_{body}$ 和磁场向量 $\mathbf{m}_{body}$ 是通过姿态将世界坐标系下的参考向量 $\mathbf{g}_{world}$ 和 $\mathbf{m}_{world}$ 旋转得到的。这个旋转操作本身就是非线性的。观测模型可以写为：

$$ \mathbf{z}_k = \begin{bmatrix} \mathbf{a}_{body} \\ \mathbf{m}_{body} \end{bmatrix} = h(\mathbf{q}_k) + \mathbf{v}_k = \begin{bmatrix} \mathbf{C}(\mathbf{q}_k)^T \mathbf{g}_{world} \\ \mathbf{C}(\mathbf{q}_k)^T \mathbf{m}_{world} \end{bmatrix} + \mathbf{v}_k $$

其中 $\mathbf{C}(\mathbf{q}_k)$ 是从四元数 $\mathbf{q}_k$ 转换得到的旋转矩阵。显然，这里的观测函数 $h(\cdot)$ 是一个复杂的非线性函数，无法用一个固定的观测矩阵 $\mathbf{H}$ 来表示。

挑战与解决思路

#

由于过程模型 $f(\cdot)$ 和观测模型 $h(\cdot)$ 都是非线性的，标准卡尔曼滤波的基石——线性假设——被打破了。高斯分布经过非线性变换后不再是高斯分布，这使得协方差矩阵的传播和更新公式失效。

为了解决这个问题，工程师和数学家们发展出了卡尔曼滤波的非线性扩展版本：

1. 扩展卡尔曼滤波 (Extended Kalman Filter, EKF)

   • 核心思想：在当前状态的估计值附近，使用泰勒级数展开将非线性函数 $f(\cdot)$ 和 $h(\cdot)$ 线性化。具体来说，是用它们的雅可比矩阵 (Jacobian Matrix) 来代替标准 KF 中的 $\mathbf{F}$ 和 $\mathbf{H}$ 矩阵。
   • 优点：实现相对简单，计算量适中。
   • 缺点：线性化会引入截断误差，仅在系统“近似线性”时效果较好。对于强非线性系统，可能会导致滤波发散。

2. 无迹卡尔曼滤波 (Unscented Kalman Filter, UKF)

   • 核心思想：放弃直接对非线性函数进行线性化，而是采用一种更巧妙的方法——无迹变换 (Unscented Transform)。它通过一组精心选择的“Sigma 点”来近似状态的概率分布，将这些点通过真实的非线性函数传递，然后重新计算变换后的均值和协方差。
   • 优点：精度通常比 EKF 高（至少到二阶），无需计算复杂的雅可比矩阵，对强非线性系统更鲁棒。
   • 缺点：计算量比 EKF 稍大，参数选择（如 Sigma 点的分布参数）需要一些经验。

总结

#

三维姿态跟踪问题是通向高级滤波算法的门户。它深刻地揭示了标准卡尔曼滤波的适用边界，并清晰地指明了学习路径：当面临非线性系统时，我们需要从 EKF 和 UKF 等工具箱中寻找解决方案。这正是卡尔曼滤波家族如此庞大且充满活力的原因所在。

结语

#

至此，我们已经完整地走过了标准卡尔曼滤波（KF）的“深入浅出”之旅。从其作为贝叶斯滤波在线性高斯系统下的特例出发，我们详细推导了预测与更新两大阶段的五个核心公式。通过一维正弦波追踪和二维平面运动跟踪这两个经典案例，我们不仅巩固了理论知识，更学会了如何将实际问题抽象、建模为卡尔曼滤波能够处理的状态空间形式。

然而，正如第三个案例“三维空间姿态跟踪”所揭示的，现实世界中充满了非线性问题。当系统的状态转移或观测过程无法用简单的矩阵乘法来描述时，标准卡尔曼滤波的线性假设便不再成立，其性能也会急剧下降甚至发散。

这恰恰是卡尔曼滤波家族精彩的延伸所在。为了应对非线性挑战，研究者们提出了多种近似最优的解决方案，其中最负盛名的便是扩展卡尔曼滤波 (Extended Kalman Filter, EKF)和无迹卡尔曼滤波 (Unscented Kalman Filter, UKF)。

希望本文为你打开了通往状态估计理论的大门。在“常见算法深入浅出”系列的下一篇文章中，我们将继续深入探索 EKF 与 UKF 的世界，解开它们在非线性系统中的滤波之谜。

参考资料

#

• 深入浅出理解卡尔曼滤波【实例、公式、代码和图】
• wikipedia-zh-卡尔曼滤波
• 从放弃到精通！卡尔曼滤波从理论到实践~
• GitHub-pykalman
• 解密卡尔曼滤波(Kalman Filter)算法：深入解析卡尔曼滤波算法原理与在线可视化实例
• DR_CAN 卡尔曼滤波器系列视频