研讨会系列 5 · Kalman Filter L2（下）

状态空间模型与协方差

从 2D 车辆的运动方程到状态空间表示——系统建模是 KF 落地的第一步。

📅 2025-11-29 ⏱ ~50 分钟 📊 中级 🎯 系统建模入门

🎯 学完本节，你应能

  • 将物理系统（如车辆运动）写为状态空间形式 $(A,B,C,D)$
  • 理解协方差矩阵的定义及其半正定性
  • 计算随机向量经线性变换后的协方差
  • 为后续 KF 测量更新中的协方差传播做准备

📖 前置知识

  • KF L1 特征值/SVD（0004）
  • 上节 MIL 矩阵求逆引理（0005a）
  • 概率论基础（期望、方差）

💡 本讲是 0005 第 2 节，与 0005a 共用同一节课的 SRT。

1. 什么是状态空间模型

状态空间模型是将动态系统写成一组一阶差分/微分方程的标准方法。离散形式：

$$ \begin{aligned} \text{状态方程:}\quad & x_{k+1} = A x_{k} + B u_{k} + w_{k} \\ \text{测量方程:}\quad & y_{k} = C x_{k} + v_{k} \end{aligned} $$

$x_{k}$

状态向量（我们想估计的东西）

$u_{k}$

控制输入（已知的）

$w_{k}$

过程噪声（随机）

$y_{k}$

测量输出（我们能观测的）

💡 为什么叫"状态空间"？状态向量 $x_{k}$ 的取值空间就是"状态空间"。系统的全部过去信息压缩在 $x_{k}$ 中——知道 $x_{k}$ 和输入，就能预测未来。

2. 【课堂例子】2D 车辆运动建模

施老师在黑板上构建了经典的 2D 车辆定位模型。

物理设定

车辆在二维平面上运动：

状态方程（位置-速度模型）

$$ \begin{bmatrix} x_{k+1} \\ v_{k+1}^x \\ y_{k+1} \\ v_{k+1}^y \end{bmatrix} = \begin{bmatrix} 1 & \Delta t & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x_{k} \\ v_{k}^x \\ y_{k} \\ v_{k}^y \end{bmatrix} + \begin{bmatrix} 0 & 0 \\ \Delta t & 0 \\ 0 & 0 \\ 0 & \Delta t \end{bmatrix} \begin{bmatrix} u_{k}^x \\ u_{k}^y \end{bmatrix} + w_{k} $$

测量方程（GPS 观测）

$$ \begin{bmatrix} \tilde{x}_k \\ \tilde{y}_k \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} \begin{bmatrix} x_{k} \\ v_{k}^x \\ y_{k} \\ v_{k}^y \end{bmatrix} + v_{k} $$

🔍 关键解读：

  • $A$ 矩阵：位置由速度积分得到（$x_{k+1} = x_{k} + \Delta t \cdot v_{k}^x$）
  • $B$ 矩阵：加速度直接改变速度，而不是位置
  • $C$ 矩阵：只能测量位置，无法直接测量速度
  • 这就是经典的"你需要从带噪位置观测中推断速度和位置"——KF 要解决的问题

3. 协方差矩阵的定义与性质

设 $x = (x_{1}, \dots, x_{n})^T$ 是一个 $n$ 维随机向量，均值 $\mu_{i} = E[x_{i}]$。

方差（对角元）

$$ \sigma_{i}^2 = E[(x_{i} - \mu_{i})^2] $$

协方差（非对角元）

$$ \sigma_{ij} = E[(x_{i} - \mu_{i})(x_{j} - \mu_{j})], \quad i \neq j $$

协方差矩阵

$$ \Sigma = \begin{bmatrix} \sigma_{1}^2 & \sigma_{12} & \cdots & \sigma_{1n} \\ \sigma_{21} & \sigma_{2}^2 & \cdots & \sigma_{2n} \\ \vdots & \vdots & \ddots & \vdots \\ \sigma_{n1} & \sigma_{n2} & \cdots & \sigma_{n}^2 \end{bmatrix} = E[(x - \mu)(x - \mu)^T] $$

💡 关键性质：协方差矩阵一定是半正定的。

对任意 $y \neq 0$：$y^T \Sigma y = E[y^T (x-\mu)(x-\mu)^T y] = E[(y^T(x-\mu))^2] \ge 0$。

这就是为什么 KF 中的 $P_{k}$（估计误差协方差）始终是半正定的——它本质上就是一个协方差矩阵。

相关性与独立性

$\sigma_{ij} = 0$ 意味着 $x_{i}$ 和 $x_{j}$ 不相关。对于高斯分布，不相关等价于独立。

4. 线性变换后的协方差传播

这是 KF 中协方差更新最核心的公式。

设 $y$ 是 $n$ 维随机向量，均值 $\mu_{y}$，协方差 $\Sigma_{y}$。对 $y$ 做线性变换：

$$ x = A y + b, \quad A \in \mathbb{R}^{m \times n},\; b \in \mathbb{R}^m $$

均值传播

$$ \mu_{x} = E[x] = E[Ay + b] = A \mu_{y} + b $$

协方差传播

$$ \Sigma_{x} = E[(x - \mu_{x})(x - \mu_{x})^T] = A \Sigma_{y} A^T $$

🔗 连接 KF：在 Kalman Filter 的时间更新中：

$$ P_{k+1|k} = A P_{k|k} A^T + Q $$

其中 $Q$ 是过程噪声 $w_{k}$ 的协方差。你看——就是协方差传播公式加上噪声项！

【数值例子】

假设位置估计误差方差 $\sigma_{x}^2 = 4$，速度估计误差方差 $\sigma_{v}^2 = 1$，两者独立：

$$ \Sigma_{y} = \begin{bmatrix} 4 & 0 \\ 0 & 1 \end{bmatrix},\quad A = \begin{bmatrix} 1 & \Delta t \end{bmatrix} $$

如果 $\Delta t = 0.1$，预测位置的方差为：

$$ \Sigma_{x} = A \Sigma_{y} A^T = \begin{bmatrix} 1 & 0.1 \end{bmatrix} \begin{bmatrix} 4 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 1 \\ 0.1 \end{bmatrix} = 4 + 0.01 = 4.01 $$

预测后不确定性增加了（因为速度的误差在时间步内累积到了位置上）。

5. 从状态空间到 Kalman Filter

完整的 KF 建模框架：

$$ \begin{aligned} x_{k+1} &= A x_{k} + B u_{k} + w_{k}, \quad w_{k} \sim \mathcal{N}(0, Q) \\ y_{k} &= C x_{k} + v_{k}, \quad v_{k} \sim \mathcal{N}(0, R) \\ E[w_{k} w_{j}^T] &= Q \delta_{kj}, \quad E[v_{k} v_{j}^T] = R \delta_{kj} \\ E[w_{k} v_{j}^T] &= 0 \quad \forall k,j \end{aligned} $$

$A$ 状态转移矩阵

描述无控系统的自然演化

$B$ 控制矩阵

输入如何影响状态

$C$ 测量矩阵

状态如何映射到观测

$Q,R$ 噪声协方差

过程噪声和测量噪声的强度

问题：我们只有带噪的 $y_{k}$，想估计真实状态 $x_{k}$。

答案：Kalman Filter 在每一步做两件事：

  1. 时间更新（预测）：用 $A$ 和 $B$ 预测下一时刻的状态，用 $A \Sigma A^T + Q$ 传播协方差
  2. 测量更新（校正）：用 MIL 高效计算 Kalman Gain，融合新测量修正估计

📌 回顾：0004 讲了特征值（理解 $A$ 的性能），0005a 讲了 MIL（高效求逆），0005b 讲了状态空间+协方差传播——这三块合在一起，就是 KF 的全部数学工具。下一讲 KF L3 开始学二次型优化和能控性。

📝 课后练习

  1. 【数值计算】考虑标量系统 $x_{k+1} = 0.9 x_{k} + w_{k}$，$w_{k} \sim \mathcal{N}(0, 1)$。若 $P_{0|0} = 1$，求 $P_{1|0}$（时间更新后的协方差）。

    查看答案

    $P_{1|0} = A P_{0|0} A^T + Q = 0.9 \cdot 1 \cdot 0.9 + 1 = 0.81 + 1 = 1.81$。

  2. 【协方差传播】二维随机向量 $y$ 的协方差 $\Sigma_{y} = \begin{bmatrix} 2 & 0.5 \\ 0.5 & 3 \end{bmatrix}$。线性变换 $A = \begin{bmatrix} 1 & -1 \\ 2 & 0 \end{bmatrix}$，求 $\Sigma_{x} = A \Sigma_{y} A^T$。

    查看答案

    先算 $A \Sigma_{y} = \begin{bmatrix} 1 & -1 \\ 2 & 0 \end{bmatrix} \begin{bmatrix} 2 & 0.5 \\ 0.5 & 3 \end{bmatrix} = \begin{bmatrix} 1.5 & -2.5 \\ 4 & 1 \end{bmatrix}$

    再乘 $A^T$: $\Sigma_{x} = \begin{bmatrix} 1.5 & -2.5 \\ 4 & 1 \end{bmatrix} \begin{bmatrix} 1 & 2 \\ -1 & 0 \end{bmatrix} = \begin{bmatrix} 4 & 3 \\ 3 & 8 \end{bmatrix}$

  3. 【推导】证明协方差传播公式 $\Sigma_{x} = A \Sigma_{y} A^T$。

    查看提示

    $x - \mu_{x} = Ay + b - (A\mu_{y} + b) = A(y - \mu_{y})$

    $\Sigma_{x} = E[(x-\mu_{x})(x-\mu_{x})^T] = E[A(y-\mu_{y})(y-\mu_{y})^T A^T] = A E[(y-\mu_{y})(y-\mu_{y})^T] A^T = A \Sigma_{y} A^T$

  4. 【建模练习】尝试为一维自由落体建模（高度 $h$，速度 $v$，重力加速度 $g$ 为已知输入）。写出 $A,B,C$ 矩阵。

    查看提示

    状态 $x = [h, v]^T$，输入 $u = g$。

    $A = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix},\; B = \begin{bmatrix} -\frac12 \Delta t^2 \\ -\Delta t \end{bmatrix},\; C = \begin{bmatrix} 1 & 0 \end{bmatrix}$（只测量高度）

📌 小结

🏗️ 状态空间

$x_{k+1} = A x_{k} + B u_{k} + w_{k}$，$y_{k} = C x_{k} + v_{k}$。任何线性系统都可这样表示。

📊 协方差传播

$x = Ay + b \Rightarrow \Sigma_{x} = A \Sigma_{y} A^T$。KF 时间更新的数学基础。

🔗 下节预告

KF L3 二次型优化与能控性：从"碗底"到"系统能否被推到目标"。

← 上一节：KF L2（上）矩阵求逆引理 🏠 返回课程馆

施老师大课堂 · 研讨会系列 5 · KF L2（下）状态空间与协方差

字幕来源：subtitles/20251129_施凌_研讨会系列5_KF_L2_audio_00742037997074f5.srt