线性动态系统中的状态估计与卡尔曼滤波

引言

在多传感器系统中，一个核心问题是：如何根据带有噪声的观测数据，对系统的真实状态进行最优估计？

当系统是静态的，即状态不随时间变化时，我们通常可以采用批处理（batch processing）或加权融合（weighted fusion）的方法来进行估计。所谓批处理，是指将一段时间内收集到的全部观测数据统一建模，并通过一次性求解最小二乘或最大似然问题来估计系统状态；所谓加权融合，则是根据不同观测源的噪声水平，为各观测分配不同权重，从而得到更可靠的综合估计。

然而在实际工程中，绝大多数系统都是动态的：系统状态随时间不断演化，而这一演化过程本身也往往伴随着不确定性。此时，观测数据不再只是对某个固定未知量的重复测量，而是对一个随时间变化的状态序列的间接反映。

这就引出了一个经典问题：

如何在一个含噪声的动态系统中，利用不完美的传感器观测，递推地估计系统的内部状态？

卡尔曼滤波器（Kalman Filter）正是解决这一问题的最优线性估计器。它在控制、导航、机器人、信号处理等领域有着极其广泛的应用。

本文基于 University of Sheffield Yuanbo Nie 老师的 ACS6124 Multisensor and Decision Systems 课程内容进行整理。作为修读这门课程的学生，我在老师的讲授与引导下获益良多，也因此希望将自己在学习过程中的一些理解与体会记录并分享出来。文中如有理解不够深入或表述不够准确之处，皆为本人学识有限所致。本文将从动态模型的作用出发，逐步推导标量卡尔曼滤波器，并进一步扩展到向量形式，讨论其背后的核心直觉。

一、动态模型在估计问题中的作用

1.1 问题引入：双足机器人的躯干姿态估计

考虑一个双足机器人行走时的姿态估计问题。

机器人的躯干相对于世界坐标系有一个俯仰角（pitch angle） $\theta(t)$ ，这是控制平衡的关键状态量。工程师在机身上安装了两种传感器：

陀螺仪（Gyroscope）：测量角速度 $\omega(t) = \dot{\theta}(t)$ ，响应快，但存在积分漂移（长时间积分后误差累积）
加速度计（Accelerometer）：可以通过重力方向间接推算倾斜角，但在机器人加速运动时读数会受到干扰

仅凭单一传感器：

只用陀螺仪积分： $\hat{\theta}(t) = \hat{\theta}_0 + \int_0^t \omega(\tau) \, d\tau$ ，短时间内精度高，但误差随时间累积，机器人走几步就会漂掉
只用加速度计：静止时估计尚可，但行走时线加速度的干扰会导致角度估计出现严重偏差

这两个传感器的误差特性是互补的——一个长时间可信，一个短时间可信。如果想同时利用两者，就必须引入一个动态模型来做融合。

1.2 动态模型如何将传感器融合起来

引入机器人躯干角度的离散时间动态模型：

状态方程（利用陀螺仪积分推进状态）：

$\theta_k = \theta_{k-1} + \Delta t \cdot \omega_{k-1} + w_{k-1}$

其中 $\Delta t$ 为采样间隔， $w_{k-1}$ 为过程噪声，反映陀螺仪积分的不确定性（漂移、噪声等）。

观测方程（加速度计提供对角度的间接观测）：

$z_k = \theta_k + v_k$

其中 $z_k$ 是由加速度计当前读数计算出的倾斜角估计， $v_k$ 为观测噪声，反映加速度计在动态运动时的测量误差。

两个关键问题在这个模型里被自然处理：

1.3 核心思想

通过这个例子可以看到，动态模型的引入意味着：

传感器误差不再是孤立的，而是在时间维度上被动力学方程关联在一起
陀螺仪提供短时高精度的状态传播，加速度计提供长时有偏但可校正的观测，两者通过动态模型形成互补
模型提供了额外的时间约束：知道”上一时刻的角度”加上”这段时间内的旋转量”就能预测”当前角度”，这一约束大幅降低了估计的不确定性

但在实际系统中，动态模型不是精确的，过程噪声和观测噪声始终存在。如何在这两类不确定性之间找到最优的融合权重——这正是卡尔曼滤波器要回答的问题。

二、问题设定：线性离散时间动态系统

2.1 系统方程

考虑如下 SISO（单输入单输出）线性时不变离散时间系统：

状态方程（State/Process Equation）：

$x_k = a \, x_{k-1} + w_{k-1}$

观测方程（Measurement/Observation Equation）：

$z_k = c \, x_k + v_k$

其中：

符号	含义
$x_k$	时刻 $k$ 的系统状态（待估计量）
$a$	状态转移系数
$c$	观测系数
$w_{k-1}$	过程噪声， $w \sim \mathcal{N}(0, \sigma_w^2)$
$v_k$	观测噪声， $v \sim \mathcal{N}(0, \sigma_v^2)$

2.2 先验知识

除了系统方程外，卡尔曼滤波器还需要以下先验信息：

过程噪声方差 $\sigma_w^2$ ：反映动态模型的不确定程度
观测噪声方差 $\sigma_v^2$ ：反映传感器的测量精度
初始状态估计 $\hat{x}_0$ 及其协方差 $\sigma_0^2$

这些先验信息的质量会直接影响滤波器的收敛速度和估计精度。

三、标量卡尔曼滤波器的推导

3.1 符号约定

在推导之前，先明确一个重要的符号约定：

$\hat{x}_{k \mid j}$

表示基于截至时刻 $j$ 的所有观测信息，对时刻 $k$ 的状态估计。

$\hat{x}_{k \mid k-1}$ ：预测值（Prior），即利用 $t_{k-1}$ 及之前的信息预测 $t_k$ 的状态
$\hat{x}_{k \mid k}$ ：更新值（Posterior），即利用包括 $t_k$ 时刻观测在内的所有信息对状态的估计

新息（Innovation / Measurement Residual）：

$\tilde{z}_k = z_k - c \, \hat{x}_{k \mid k-1}$

新息反映的是实际观测与基于模型预测的观测之间的差异。

3.2 推导思路

卡尔曼滤波器的推导目标是找到一个最优的线性更新规则。假设更新步骤的形式为：

$\hat{x}_{k \mid k} = \hat{x}_{k \mid k-1} + K_k \, \tilde{z}_k$

其中 $K_k$ 称为卡尔曼增益（Kalman Gain），是待确定的参数。

推导的核心步骤为：

定义估计误差： $e_{k \mid k} = x_k - \hat{x}_{k \mid k}$
构造误差的方差（即估计误差协方差）： $\sigma_{k \mid k}^2 = E[e_{k \mid k}^2]$
对 $\sigma_{k \mid k}^2$ 关于 $K_k$ 求导并令其为零
解出使估计误差方差最小的最优 $K_k$

这一过程的本质是：在均方误差最小（Minimum Mean Square Error, MMSE）意义下，找到最优的融合权重。

3.3 卡尔曼增益

经推导，最优卡尔曼增益为：

$K_k = \frac{c \, \sigma_{k \mid k-1}^2}{c^2 \, \sigma_{k \mid k-1}^2 + \sigma_v^2}$

其中 $\sigma_{k \mid k-1}^2$ 是预测步骤的状态协方差。

3.4 标量卡尔曼滤波器完整算法

给定线性时不变离散时间系统：

$x_k = a \, x_{k-1} + w_{k-1}, \quad z_k = c \, x_k + v_k$

以及先验知识 $\sigma_w^2$ ， $\sigma_v^2$ ， $\hat{x}_0$ ， $\sigma_0^2$ 。

算法步骤（对每个时间步 $k = 1, 2, 3, \ldots$ ）：

Step 1：预测（Prediction）

$\hat{x}_{k \mid k-1} = a \, \hat{x}_{k-1 \mid k-1}$ $\sigma_{k \mid k-1}^2 = a^2 \, \sigma_{k-1 \mid k-1}^2 + \sigma_w^2$

Step 2：计算卡尔曼增益

$K_k = \frac{c \, \sigma_{k \mid k-1}^2}{c^2 \, \sigma_{k \mid k-1}^2 + \sigma_v^2}$

Step 3：更新（Correction）

$\hat{x}_{k \mid k} = \hat{x}_{k \mid k-1} + K_k \left( z_k - c \, \hat{x}_{k \mid k-1} \right)$ $\sigma_{k \mid k}^2 = (1 - K_k \, c) \, \sigma_{k \mid k-1}^2$

四、卡尔曼滤波器的直觉理解

4.1 三步循环：预测 → 观测 → 融合

卡尔曼滤波器的运行可以直观地理解为一个在每个时间步重复执行的三步循环：

第一步：预测（Prediction）

利用动态模型，将上一时刻的状态估计”推进”到当前时刻。这一步本质上是对无噪声动力学方程的前向仿真：

$\hat{x}_{k \mid k-1} = a \, \hat{x}_{k-1 \mid k-1}$

预测会产生一个新的估计值和一个增大的不确定性（因为过程噪声的累积）。

第二步：观测（Measurement）

在当前时刻获取新的传感器测量值 $z_k$ 。这个测量值通过观测模型与状态关联：

$z_k = c \, x_k + v_k$

观测值同样带有自身的不确定性（由 $\sigma_v^2$ 表征）。

第三步：融合/校正（Fusion / Correction）

将模型预测和传感器观测进行最优融合，得到更新后的估计：

$\hat{x}_{k \mid k} = \hat{x}_{k \mid k-1} + K_k (z_k - c \, \hat{x}_{k \mid k-1})$

融合的结果总是比单独使用预测或观测更精确——更新后的不确定性 $\sigma_{k \mid k}^2$ 总是小于预测不确定性 $\sigma_{k \mid k-1}^2$ 。

4.2 卡尔曼增益的直觉

卡尔曼增益 $K_k$ 决定了更新步骤中”更信任预测”还是”更信任观测”：

情况一：观测不确定性 >> 预测不确定性

$\sigma_v^2 \gg \sigma_{k \mid k-1}^2 \implies K_k \approx 0$

此时卡尔曼增益很小，更新值几乎等于预测值。直觉上就是：传感器太不靠谱了，不如信模型。

情况二：预测不确定性 >> 观测不确定性

$\sigma_{k \mid k-1}^2 \gg \sigma_v^2 \implies K_k \approx \frac{1}{c}$

此时卡尔曼增益很大，更新值几乎完全偏向观测。直觉上就是：模型预测的不确定性太大了，还是信传感器。

这种自适应的加权融合机制，正是卡尔曼滤波器优雅之处——它会根据预测和观测各自的不确定性，自动调整信任比例。

五、向量卡尔曼滤波器

5.1 系统方程的向量形式

在更一般的 MIMO（多输入多输出）场景中，系统方程扩展为：

状态方程：

$\mathbf{x}_k = \mathbf{A} \, \mathbf{x}_{k-1} + \mathbf{B} \, \mathbf{u}_{k-1} + \mathbf{G} \, \mathbf{w}_{k-1}$

观测方程：

$\mathbf{z}_k = \mathbf{C} \, \mathbf{x}_k + \mathbf{v}_k$

其中：

符号	维度	含义
$\mathbf{x}_k$	$n \times 1$	状态向量
$\mathbf{A}$	$n \times n$	状态转移矩阵
$\mathbf{B}$	$n \times m$	输入矩阵
$\mathbf{u}_k$	$m \times 1$	控制输入
$\mathbf{G}$	$n \times p$	噪声输入矩阵
$\mathbf{w}_k$	$p \times 1$	过程噪声， $\mathbf{w} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q})$
$\mathbf{z}_k$	$q \times 1$	观测向量
$\mathbf{C}$	$q \times n$	观测矩阵
$\mathbf{v}_k$	$q \times 1$	观测噪声， $\mathbf{v} \sim \mathcal{N}(\mathbf{0}, \mathbf{R})$

注意与标量情况的区别：这里噪声输入矩阵 $\mathbf{G}$ 不再限制为单位矩阵，这使得模型更加通用。

5.2 先验知识

过程噪声协方差矩阵 $\mathbf{Q}$
观测噪声协方差矩阵 $\mathbf{R}$
初始状态估计 $\hat{\mathbf{x}}_0$ 及初始协方差矩阵 $\mathbf{P}_0$

5.3 向量卡尔曼滤波器算法

对每个时间步 $k = 1, 2, 3, \ldots$ ：

Step 1：状态预测

$\hat{\mathbf{x}}_{k \mid k-1} = \mathbf{A} \, \hat{\mathbf{x}}_{k-1 \mid k-1} + \mathbf{B} \, \mathbf{u}_{k-1}$

这一步本质上是对无噪声动力学方程的前向仿真。

Step 2：协方差预测

$\mathbf{P}_{k \mid k-1} = \mathbf{A} \, \mathbf{P}_{k-1 \mid k-1} \, \mathbf{A}^T + \mathbf{G} \, \mathbf{Q} \, \mathbf{G}^T$

预测协方差反映了状态预测的不确定性，由上一时刻的不确定性经 $\mathbf{A}$ 传播后，再加上过程噪声的贡献。

Step 3：观测预测

$\hat{\mathbf{z}}_{k \mid k-1} = \mathbf{C} \, \hat{\mathbf{x}}_{k \mid k-1}$

这一步是对无噪声观测方程的计算。

Step 4：新息协方差

$\mathbf{S}_k = \mathbf{C} \, \mathbf{P}_{k \mid k-1} \, \mathbf{C}^T + \mathbf{R}$

将状态空间中的不确定性映射到观测空间。

Step 5：获取新观测

在时刻 $t_k$ 获取传感器测量值 $\mathbf{z}_k$ 。

Step 6：状态校正

$\mathbf{K}_k = \mathbf{P}_{k \mid k-1} \, \mathbf{C}^T \, \mathbf{S}_k^{-1}$ $\hat{\mathbf{x}}_{k \mid k} = \hat{\mathbf{x}}_{k \mid k-1} + \mathbf{K}_k \left( \mathbf{z}_k - \hat{\mathbf{z}}_{k \mid k-1} \right)$

Step 7：协方差更新

$\mathbf{P}_{k \mid k} = (\mathbf{I} - \mathbf{K}_k \, \mathbf{C}) \, \mathbf{P}_{k \mid k-1}$

六、卡尔曼滤波器的工程要点与讨论

6.1 噪声参数的影响

在实际应用中， $\mathbf{Q}$ 和 $\mathbf{R}$ 的选择对滤波性能有决定性影响：

$\mathbf{Q}$ 偏大：滤波器认为模型不可信，会更多地依赖观测数据。滤波结果对测量噪声更敏感，估计值跟踪观测值更紧密，但同时也更容易受噪声干扰。
$\mathbf{Q}$ 偏小：滤波器认为模型很可信，会更多地依赖动态预测。估计值更平滑，但对真实状态变化的响应会变慢。
$\mathbf{R}$ 偏大：滤波器认为传感器不可信，卡尔曼增益变小，更信任模型预测。
$\mathbf{R}$ 偏小：滤波器认为传感器很精确，卡尔曼增益变大，估计值紧贴观测。

6.2 卡尔曼滤波器的最优性条件

卡尔曼滤波器在以下条件下是最优线性无偏估计器（Best Linear Unbiased Estimator, BLUE）：

系统是线性的
噪声是高斯白噪声
过程噪声和观测噪声相互独立
噪声的统计特性（ $\mathbf{Q}$ , $\mathbf{R}$ ）是准确已知的
初始状态的先验信息是正确的

当这些条件满足时，卡尔曼滤波器不仅是所有线性估计器中最优的，也是所有估计器（包括非线性）中最优的。

当系统为非线性时，需要使用扩展卡尔曼滤波器（Extended Kalman Filter, EKF）或其他非线性滤波方法，这将在后续 Lecture 中讨论。

七、总结

卡尔曼滤波器的核心思想可以概括为以下几点：

7.1 算法本质

卡尔曼滤波器是一个递推贝叶斯估计器。在每个时间步，它执行两个基本操作：

预测（Predict）：利用系统动态模型将上一时刻的后验估计推进到当前时刻，得到先验估计
更新（Update）：利用当前时刻的观测数据对先验估计进行校正，得到后验估计

7.2 核心公式速查

状态预测

$\hat{x}_{k \mid k-1} = a \hat{x}_{k-1 \mid k-1}$ $\hat{\mathbf{x}}_{k \mid k-1} = \mathbf{A}\hat{\mathbf{x}}_{k-1 \mid k-1} + \mathbf{B}\mathbf{u}_{k-1}$

协方差预测

$\sigma_{k \mid k-1}^2 = a^2 \sigma_{k-1 \mid k-1}^2 + \sigma_w^2$ $\mathbf{P}_{k \mid k-1} = \mathbf{A}\mathbf{P}_{k-1 \mid k-1}\mathbf{A}^T + \mathbf{G}\mathbf{Q}\mathbf{G}^T$

卡尔曼增益

$K_k = \frac{c\sigma_{k \mid k-1}^2}{c^2\sigma_{k \mid k-1}^2 + \sigma_v^2}$ $\mathbf{K}_k = \mathbf{P}_{k \mid k-1}\mathbf{C}^T\mathbf{S}_k^{-1}$

状态更新

$\hat{x}_{k \mid k} = \hat{x}_{k \mid k-1} + K_k(z_k - c\hat{x}_{k \mid k-1})$ $\hat{\mathbf{x}}_{k \mid k} = \hat{\mathbf{x}}_{k \mid k-1} + \mathbf{K}_k(\mathbf{z}_k - \mathbf{C}\hat{\mathbf{x}}_{k \mid k-1})$

协方差更新

$\sigma_{k \mid k}^2 = (1 - K_k c)\sigma_{k \mid k-1}^2$ $\mathbf{P}_{k \mid k} = (\mathbf{I} - \mathbf{K}_k\mathbf{C})\mathbf{P}_{k \mid k-1}$

7.3 关键直觉

动态模型提供了时间维度上的信息约束，使得欠定的估计问题变得可解
卡尔曼增益自适应地平衡模型预测与传感器观测：模型不确定性大时信传感器，传感器不确定性大时信模型
协方差的递推传播是卡尔曼滤波器的灵魂所在——它不仅估计状态，还精确地量化了估计的不确定性
预测步骤增大不确定性（过程噪声的累积），更新步骤减小不确定性（融合观测信息），两者交替进行形成稳定的估计循环

7.4 适用条件与局限

条件	说明
线性系统	状态方程和观测方程均为线性
高斯噪声	过程噪声和观测噪声为零均值高斯分布
已知噪声参数	$\mathbf{Q}$ 和 $\mathbf{R}$ 需要准确或近似已知
独立噪声	过程噪声和观测噪声相互独立

当上述条件不完全满足时（尤其是非线性系统），需要使用 EKF、UKF 或粒子滤波器等扩展方法。