ch03_linear_gaussian_optimized

第三章　线性高斯估计（优化版） §

Chapter 3   Linear-Gaussian Estimation (Optimized) §

---

本章导航 / Chapter Navigator §

一句话总结（TL;DR）

当运动和观测都是线性的、噪声都是高斯的时候，最优估计有闭式解。从”一次性算所有时刻”的批量方法，可以分解出”一步步往前算”的卡尔曼滤波器。

阅读路径建议：

• 🟢 快速浏览：读完”直觉图解”即可把握核心思想
• 🔵 理解算法：阅读”概念层”的公式和解释
• ⚫ 深度推导：展开”推导层”查看完整数学

---

3.1 批量估计：交卷前可以回头检查 §

🎯 直觉：为什么要”批量”？ §

想象你在做一道多步计算题：

• 在线估计（滤波器）：做完第3步就不能改了，继续往后做
• 批量估计（平滑器）：交卷前可以回头修改任何一步，因为你知道后面的答案也能帮助判断前面是否正确

关键洞察：如果允许使用”未来的测量”来修正”过去的估计”，精度会更高。

---

🔑 概念层：问题设置 §

我们有一个随时间移动的东西（比如机器人），用离散时刻 $k=0,1,2,\dots ,K$ 描述。

运动模型（即使没有观测，我们也大概知道它会怎么动）：

${\mathbf{x}}_k\approx {\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{v}}_k$

观测模型（传感器告诉我们一些关于状态的信息）：

${\mathbf{y}}_k\approx {\mathbf{C}}_k{\mathbf{x}}_k$

符号速查：$\mathbf{x}$ 是状态（位置/速度等），$\mathbf{y}$ 是观测，$\mathbf{A}$ 是状态转移矩阵，$\mathbf{C}$ 是观测矩阵。波浪号 $\approx$ 表示”大约等于”（因为有噪声）。

目标：给定所有观测 ${\mathbf{y}}_0,\dots ,{\mathbf{y}}_K$ 和初始猜测，估计所有时刻的状态 ${\mathbf{x}}_0,\dots ,{\mathbf{x}}_K$。

---

📐 推导层：最小二乘求解 §

展开：如何将问题转化为线性系统

把所有状态堆成一个大向量：$\mathbf{x}=[{\mathbf{x}}_0^T,{\mathbf{x}}_1^T,\dots ,{\mathbf{x}}_K^T{]}^T$

把所有方程（运动 + 观测）写成矩阵形式：

$\mathbf{z}=\mathbf{Hx}+\text{噪声}$

其中 $\mathbf{z}$ 包含所有观测和初始先验，$\mathbf{H}$ 是稀疏矩阵。

MAP 估计等价于最小化：

$J(\mathbf{x})=\frac{1}{2}(\mathbf{z}-\mathbf{Hx}{)}^T{\mathbf{W}}^{-1}(\mathbf{z}-\mathbf{Hx})$

对 $\mathbf{x}$ 求导并令为零：

$\underset{\mathbf{A}}{\underbrace{{\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H}}}{\mathbf{x}}^{\ast }=\underset{\mathbf{b}}{\underbrace{{\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{z}}}$

这就是著名的正规方程（Normal Equations）。

关键观察：$\mathbf{A}$ 是块三对角矩阵（每个时刻只与相邻时刻耦合），这使得求解可以高效进行。

---

3.2 Cholesky 平滑器：稀疏性的力量 §

🎯 直觉：为什么批量估计可以很快？ §

$K$ 个时刻，每个状态 $N$ 维，总共有 $N(K+1)$ 个未知数。直接求逆矩阵的复杂度是 $O((NK{)}^3)$——当 $K=1000$ 时根本算不完。

但物理世界有个特性：时刻 $k$ 只与 $k-1$ 和 $k+1$ 直接相关（马尔可夫性）。这导致 $\mathbf{A}$ 矩阵非常稀疏——大部分是零！

形象地说，$\mathbf{A}$ 长这样（空白=零）：

$\mathbf{A}=\left[\begin{array}{ccccc}\times & \times & & & \\ \times & \times & \times & & \\ & \times & \times & \times & \\ & & \times & \times & \times \\ & & & \times & \times \end{array}\right]$

这叫做块三对角结构。

---

🔑 概念层：两步求解法 §

利用 Cholesky 分解 $\mathbf{A}=\mathbf{L}{\mathbf{L}}^T$，其中 $\mathbf{L}$ 是下三角矩阵。

第一步：前向传递（求 $\mathbf{d}$）

$\mathbf{Ld}=\mathbf{b}$

由于 $\mathbf{L}$ 是下三角，可以从上到下依次求解（像多米诺骨牌）。

第二步：后向传递（求 ${\mathbf{x}}^{\ast }$）

${\mathbf{L}}^T{\mathbf{x}}^{\ast }=\mathbf{d}$

由于是上三角，可以从下到上依次求解。

复杂度：$O(N^{3}K)$ —— 与时刻数 $K$ 成线性关系！比直接求逆快 $K^2$ 倍。

---

📐 推导层：前向-后向算法 §

展开：块三对角系统的递归求解

设 $\mathbf{A}$ 的块三对角分解为：

$\mathbf{L}=\left[\begin{array}{cccc}{\mathbf{L}}_0& & & \\ {\mathbf{F}}_0& {\mathbf{L}}_1& & \\ & {\mathbf{F}}_1& {\mathbf{L}}_2& \\ & & \ddots & \ddots \end{array}\right]$

前向传递（Forward Pass）：

对每个 $k=0,1,\dots ,K$：

${\mathbf{L}}_k{\mathbf{L}}_k^T={\mathbf{W}}_k+{\mathbf{F}}_{k-1}{\mathbf{F}}_{k-1}^T\text{(Cholesky分解)}$

${\mathbf{d}}_k={\mathbf{L}}_k^{-1}({\mathbf{b}}_k-{\mathbf{F}}_{k-1}{\mathbf{d}}_{k-1})$

后向传递（Backward Pass）：

从 $k=K$ 到 $k=0$：

${\mathbf{x}}_k^{\ast }={\mathbf{L}}_k^{-T}{\mathbf{d}}_k-{\mathbf{L}}_k^{-T}{\mathbf{F}}_k^T{\mathbf{x}}_{k+1}^{\ast }$

这就是 Cholesky 平滑器的完整算法。

---

3.3 RTS 平滑器：协方差形式更直观 §

🎯 直觉：Cholesky 的”协方差版本” §

Cholesky 平滑器工作在”信息域”（处理 $\mathbf{A}={\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H}$）。

但更常见的是用协方差 $\mathbf{P}$ 思考——“我的估计误差有多大？”

RTS 平滑器与 Cholesky 平滑器数学等价，但全用协方差表示，更直观。

---

🔑 概念层：两步算法 §

第一步：前向滤波（卡尔曼滤波器）

从 $k=0$ 到 $K$，计算并存储：

• 先验估计 $({\check{\mathbf{x}}}_k,{\check{\mathbf{P}}}_k)$：基于过去所有数据
• 后验估计 $({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k)$：再加上当前观测

第二步：后向平滑（用未来数据修正过去）

从 $k=K-1$ 到 $k=0$：

$\text{平滑增益：}{\mathbf{G}}_k={\hat{\mathbf{P}}}_k{\mathbf{A}}_k^T{\check{\mathbf{P}}}_{k+1}^{-1}$

$\text{修正估计：}{\hat{\mathbf{x}}}_k^s={\hat{\mathbf{x}}}_k+{\mathbf{G}}_k({\hat{\mathbf{x}}}_{k+1}^s-{\check{\mathbf{x}}}_{k+1})$

$\text{修正协方差：}{\hat{\mathbf{P}}}_k^s={\hat{\mathbf{P}}}_k+{\mathbf{G}}_k({\hat{\mathbf{P}}}_{k+1}^s-{\check{\mathbf{P}}}_{k+1}){\mathbf{G}}_k^T$

直观理解：

• ${\hat{\mathbf{x}}}_{k+1}^s-{\check{\mathbf{x}}}_{k+1}$：未来数据揭示的”预测误差”
• ${\mathbf{G}}_k$：我们该相信这个误差多少？（如果运动模型很准，就别太信；如果运动模型不准，就多信）

---

🧮 数值示例：一维运动 §

假设一个物体沿直线运动，我们观测它的位置。

设置：

• 真实位置：时刻0在0，时刻1在10，时刻2在20（匀速运动）
• 观测噪声：标准差 2
• 运动模型假设：每步大约移动 10，但不确定（标准差 3）

前向滤波结果：

时刻	观测	滤波估计	滤波标准差
0	1.2	1.2	2.0
1	8.5	9.1	1.6
2	22.3	20.5	1.4

平滑后结果：

时刻	平滑估计	平滑标准差
0	0.8	1.2
1	10.2	1.1
2	20.5	1.4

观察：

• 时刻0的估计从 1.2 修正到 0.8（更接近真实值0）
• 时刻0的不确定性从 2.0 降到 1.2（用了未来信息）
• 时刻2是终点，没有未来信息，所以不变

---

📐 推导层：从 Cholesky 到 RTS §

展开：两种平滑器的等价性证明

RTS 平滑器的后向传递可以写成：

${\hat{\mathbf{x}}}_{k-1}^s={\hat{\mathbf{x}}}_{k-1}+{\mathbf{K}}_{k-1}({\hat{\mathbf{x}}}_k^s-{\check{\mathbf{x}}}_k)$

其中 ${\mathbf{K}}_{k-1}={\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_{k-1}^T{\check{\mathbf{P}}}_k^{-1}$。

这等价于 Cholesky 平滑器的后向传递：

${\mathbf{x}}_{k-1}^{\ast }={\mathbf{L}}_{k-1}^{-T}{\mathbf{d}}_{k-1}-{\mathbf{L}}_{k-1}^{-T}{\mathbf{F}}_{k-1}^T{\mathbf{x}}_k^{\ast }$

通过 SMW（Sherman-Morrison-Woodbury）恒等式可以证明两者代数等价。

关键观察：

• Cholesky：工作在信息矩阵域，数值更稳定
• RTS：工作在协方差域，更直观易懂
• 两者给出完全相同的估计结果

---

3.4 卡尔曼滤波器：只往前看的在线估计 §

🎯 直觉：为什么需要”滤波器”？ §

前面的平滑器都需要”交卷后回头检查”——但在很多场景下，我们必须实时做决策：

• 自动驾驶汽车：不能等到旅程结束才知道5秒前在哪里
• 火箭导航：不能等到着陆后再修正轨道
• 机器人避障：必须现在就知道当前位置

卡尔曼滤波器就是 RTS 平滑器的前向部分——只用过去和现在的数据，不用未来数据。

---

🔑 概念层：预测-校正循环 §

每个时刻，重复两个步骤：

1. 预测（Prediction）

根据运动模型，从上一时刻预测当前时刻：

${\check{\mathbf{x}}}_k={\mathbf{A}}_{k-1}{\hat{\mathbf{x}}}_{k-1}+{\mathbf{v}}_k$

${\check{\mathbf{P}}}_k={\mathbf{A}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_{k-1}^T+{\mathbf{Q}}_k$

直观：运动模型告诉我们”应该在哪里”，但不确定性会累积（$+{\mathbf{Q}}_k$）

2. 校正（Correction）

拿到观测后，融合预测和观测：

${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{C}}_k^T({\mathbf{C}}_k{\check{\mathbf{P}}}_k{\mathbf{C}}_k^T+{\mathbf{R}}_k{)}^{-1}$

${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k({\mathbf{y}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_k)$

${\hat{\mathbf{P}}}_k=(1-{\mathbf{K}}_k{\mathbf{C}}_k){\check{\mathbf{P}}}_k$

直观：

• ${\mathbf{y}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_k$：观测与预测的差距（新息 Innovation）
• ${\mathbf{K}}_k$（卡尔曼增益）：该信预测还是信观测？如果观测噪声小（${\mathbf{R}}_k$ 小），就多信观测

---

🧮 数值示例：跟踪飞行中的球 §

假设你扔一个球，用摄像头观测它的位置。

状态：$\mathbf{x}=[p,v{]}^T$（位置，速度）

运动模型（重力作用）：

${\mathbf{x}}_k=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]{\mathbf{x}}_{k-1}+\left[\begin{array}{c}0\\ -g\Delta t\end{array}\right]+\text{噪声}$

观测模型（只看到位置）：

$y_k=\left[\begin{array}{cc}1& 0\end{array}\right]{\mathbf{x}}_k+\text{噪声}$

卡尔曼滤波的神奇之处：

• 即使只观测位置，滤波器也能自动估计出速度
• 速度估计不需要显式微分（微分会放大噪声）
• 这是通过运动模型的物理约束”隐式”估计的

---

📐 推导层：卡尔曼滤波器的五种等价形式 §

展开：不同形式的卡尔曼增益

形式1（信息形式）： ${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{C}}_k^T({\mathbf{C}}_k{\check{\mathbf{P}}}_k{\mathbf{C}}_k^T+{\mathbf{R}}_k{)}^{-1}$

形式2（协方差更新形式）： ${\hat{\mathbf{P}}}_k^{-1}={\check{\mathbf{P}}}_k^{-1}+{\mathbf{C}}_k^T{\mathbf{R}}_k^{-1}{\mathbf{C}}_k$

形式3（Joseph形式，数值稳定）： ${\hat{\mathbf{P}}}_k=(1-{\mathbf{K}}_k{\mathbf{C}}_k){\check{\mathbf{P}}}_k(1-{\mathbf{K}}_k{\mathbf{C}}_k{)}^T+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^T$

这些形式通过矩阵求逆引理相互等价。实际应用中根据维度和数值稳定性选择。

---

3.5 三种方法的统一视角 §

🎯 核心结论 §

方法	信息使用	计算复杂度	应用场景
批量估计	过去+未来	$O(N^{3}K)$	离线分析、地图构建
RTS平滑器	过去+未来	$O(N^{3}K)$	需要协方差形式的离线分析
卡尔曼滤波器	仅过去	$O(N^3)$ 每步	实时估计、在线控制

关键洞察：

1. 它们都是同一个优化问题的解
2. 区别只在于”能否用未来数据”
3. 如果噪声是高斯的、模型是线性的，这些方法都是最优的（MVU：最小方差无偏）

---

本章自检清单 §

阅读完本章，你应该能回答：

• 为什么批量估计比逐点估计更准？（因为用了未来信息）
• 为什么 $O((NK{)}^3)$ 可以优化到 $O(N^{3}K)$？（利用块三对角稀疏性）
• 卡尔曼增益 ${\mathbf{K}}_k$ 的直观意义是什么？（预测 vs 观测的信任分配）
• 平滑器和滤波器的本质区别是什么？（能否用未来数据）

如果以上都清楚，恭喜你掌握了线性高斯估计的核心！

---

延伸阅读与代码 §

Python 实现（伪代码）：

def kalman_filter(x0, P0, A, C, Q, R, measurements):
    """
    线性卡尔曼滤波器
    """
    x = x0  # 初始状态估计
    P = P0  # 初始协方差
    estimates = []
 
    for y in measurements:
        # 预测
        x_pred = A @ x
        P_pred = A @ P @ A.T + Q
 
        # 校正
        S = C @ P_pred @ C.T + R  # 新息协方差
        K = P_pred @ C.T @ np.linalg.inv(S)  # 卡尔曼增益
        x = x_pred + K @ (y - C @ x_pred)  # 状态更新
        P = (np.eye(len(x)) - K @ C) @ P_pred  # 协方差更新
 
        estimates.append(x)
 
    return estimates
 
 
def rts_smoother(filter_estimates, filter_covariances, A, Q):
    """
    RTS 后向平滑器
    """
    K = len(filter_estimates)
    x_smooth = [None] * K
    P_smooth = [None] * K
 
    # 从最后一个时刻开始
    x_smooth[-1] = filter_estimates[-1]
    P_smooth[-1] = filter_covariances[-1]
 
    # 后向传递
    for k in range(K-2, -1, -1):
        # 预测（前向滤波时存储的）
        x_pred = A @ filter_estimates[k]
        P_pred = A @ filter_covariances[k] @ A.T + Q
 
        # 平滑增益
        G = filter_covariances[k] @ A.T @ np.linalg.inv(P_pred)
 
        # 修正
        x_smooth[k] = filter_estimates[k] + G @ (x_smooth[k+1] - x_pred)
        P_smooth[k] = filter_covariances[k] + G @ (P_smooth[k+1] - P_pred) @ G.T
 
    return x_smooth, P_smooth

---

下一章将进入非线性非高斯世界，这些闭式解不再适用，需要近似方法。