ch04_nonlinear_optimized

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

Chapter 4   Nonlinear Non-Gaussian Estimation (Optimized) §

---

本章导航 / Chapter Navigator §

一句话总结（TL;DR）

当模型是非线性的，或者噪声不是高斯的，我们失去了闭式解。策略是：线性化（EKF）、采样（粒子滤波）、或者直接优化（批量MAP）。

阅读路径建议：

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

---

4.1 为什么非线性让问题变难？ §

🎯 直觉：高斯分布经过非线性变换后不再是高斯 §

想象你有一个钟形曲线（高斯分布）：

• 线性变换：把它拉长或压缩，还是钟形
• 非线性变换：比如取平方，钟形会被”扭曲”

关键问题：卡尔曼滤波器假设状态永远是高斯分布。如果运动方程是非线性的， propagated 后的分布可能变成任意形状——卡尔曼的假设就崩塌了。

---

🔑 概念层：三种应对策略 §

策略	核心思想	优点	缺点
线性化 (EKF)	在工作点附近泰勒展开	计算快	非线性强时发散
采样 (粒子滤波)	用一堆样本近似分布	能处理任意分布	维度灾难
直接优化 (MAP)	不管分布形状，直接找最优点	精度高	可能陷入局部极小

本章路线图：先讲线性化（EKF），再讲采样（粒子滤波），最后讲批量优化（MAP）。

---

4.2 扩展卡尔曼滤波器 (EKF) §

🎯 直觉：用切线近似曲线 §

想象你要跟踪一辆车：

• 真实运动：沿着弯曲的道路行驶（非线性）
• EKF 的做法：在”估计位置”处画一条切线，假设车沿着切线走一小步

只要步长够小，切线近似就够准。但如果道路很弯、或者步长太大，车就会”跑偏”。

---

🔑 概念层：线性化核心操作 §

非线性运动模型： ${\mathbf{x}}_k=\mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{v}}_k)+{\mathbf{w}}_k$

在工作点 ${\mathbf{x}}_{\mathrm{op}}$ 附近泰勒展开： $\mathbf{f}(\mathbf{x})\approx \mathbf{f}({\mathbf{x}}_{\mathrm{op}})+\underset{\mathbf{F}}{\underbrace{\frac{\partial \mathbf{f}}{\partial \mathbf{x}}{|}_{{\mathbf{x}}_{\mathrm{op}}}}}(\mathbf{x}-{\mathbf{x}}_{\mathrm{op}})$

$\mathbf{F}$ 是什么？：状态转移的”局部斜率矩阵”。如果状态是位置+速度，$\mathbf{F}$ 告诉你”当前位置和速度如何影响下一时刻的位置和速度”。

EKF 的两步循环：

预测： ${\check{\mathbf{x}}}_k=\mathbf{f}({\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k)$ ${\check{\mathbf{P}}}_k={\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^T+{\mathbf{Q}}_k$

校正： ${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{H}}_k^T({\mathbf{H}}_k{\check{\mathbf{P}}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k{)}^{-1}$ ${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k({\mathbf{y}}_k-\mathbf{h}({\check{\mathbf{x}}}_k))$

与线性卡尔曼滤波的唯一区别：

• 用 $\mathbf{f}(\cdot )$ 代替 $\mathbf{Ax}+\mathbf{v}$
• 用 $\mathbf{h}(\cdot )$ 代替 $\mathbf{Cx}$
• 用雅可比矩阵 $\mathbf{F},\mathbf{H}$ 代替常数矩阵 $\mathbf{A},\mathbf{C}$

---

🧮 数值示例：跟踪圆周运动的物体 §

场景：物体以原点为中心做圆周运动，我们只能观测距离（而非角度）。

真实运动： $x_k=\cos (k\cdot 0.1),y_k=\sin (k\cdot 0.1)$

观测（距离原点）： $r_k=\sqrt{x_k^2+y_k^2}+\text{噪声}$

EKF 的工作：

1. 预测：假设物体沿当前速度方向走直线
2. 校正：根据距离观测调整位置

为什么会发散？

• 物体实际上在转弯，但 EKF 假设它走直线
• 长时间后，预测位置偏离真实轨道
• 如果距离观测不能很好约束角度，估计就会”漂移”

---

📐 推导层：泰勒展开的误差分析 §

展开：二阶项何时不能忽略？

二阶泰勒展开： $\mathbf{f}(\mathbf{x})\approx \mathbf{f}({\mathbf{x}}_{\mathrm{op}})+\mathbf{F}(\mathbf{x}-{\mathbf{x}}_{\mathrm{op}})+\frac{1}{2}{\sum }_i{\mathbf{e}}_i(\mathbf{x}-{\mathbf{x}}_{\mathrm{op}}{)}^T{\mathbf{G}}_i(\mathbf{x}-{\mathbf{x}}_{\mathrm{op}})$

其中 ${\mathbf{G}}_i$ 是 Hessian 矩阵。

EKF 忽略二阶项的条件： $\Vert {\mathbf{G}}_i\Vert \cdot \Vert \check{\mathbf{P}}\Vert \ll 1$

即：曲率小 或 不确定性小。

实际建议：

• 如果系统高度非线性（如转弯很急的机器人），考虑 UKF 或粒子滤波
• 如果更新频率高（步长小），EKF 通常足够

---

4.3 批量 MAP 估计：直接优化 §

🎯 直觉：与其一步步近似，不如整体找最优 §

EKF 是”在线”的——每步只做局部最优决策。

但如果允许”离线”处理（比如建图），我们可以：

1. 把所有时刻的状态放在一起
2. 定义一个”总误差”（目标函数）
3. 用优化算法直接最小化

类比：

• EKF 像贪心算法——每步做局部最优
• MAP 像动态规划——考虑整体最优

---

🔑 概念层：目标函数的构建 §

MAP（最大后验）目标： $J(\mathbf{x})=\underset{\text{运动误差}}{\underbrace{\frac{1}{2}\sum _k{\mathbf{e}}_{v,k}^T{\mathbf{Q}}_k^{-1}{\mathbf{e}}_{v,k}}}+\underset{\text{观测误差}}{\underbrace{\frac{1}{2}\sum _k{\mathbf{e}}_{y,k}^T{\mathbf{R}}_k^{-1}{\mathbf{e}}_{y,k}}}$

其中：

• ${\mathbf{e}}_{v,k}={\mathbf{x}}_k-\mathbf{f}({\mathbf{x}}_{k-1})$：“实际运动与模型预测的差异”
• ${\mathbf{e}}_{y,k}={\mathbf{y}}_k-\mathbf{h}({\mathbf{x}}_k)$：“观测与预测的差距”

高斯-牛顿法：

1. 在当前猜测 ${\mathbf{x}}_{\mathrm{op}}$ 处线性化
2. 解线性最小二乘问题得到修正量 $\delta {\mathbf{x}}^{\ast }$
3. 更新：${\mathbf{x}}_{\mathrm{op}}\leftarrow {\mathbf{x}}_{\mathrm{op}}+\delta {\mathbf{x}}^{\ast }$
4. 重复直到收敛

与 EKF 的区别：

• EKF：线性化一次，走一步
• GN：反复线性化直到收敛（迭代）

---

🧮 数值示例：用 GN 拟合正弦曲线 §

问题：观测到带噪声的点 $(t_i,y_i)$，拟合模型 $y=A\sin (\omega t+\varphi )$。

参数：$\mathbf{x}=[A,\omega ,\varphi {]}^T$（非线性！）

GN 迭代过程：

迭代	$A$	$\omega$	$\varphi$	残差
0 (初值)	1.0	1.0	0.0	大
1	1.2	0.9	0.3	中
2	1.05	1.0	0.1	小
3	1.01	1.0	0.02	很小

观察：

• 即使初值偏差较大，GN 也能收敛
• 但如果初值太差（比如 $\omega =5$），可能收敛到错误解

---

📐 推导层：高斯-牛顿法的推导 §

展开：从泰勒到正规方程

线性化残差： $\mathbf{e}(\mathbf{x})\approx \mathbf{e}({\mathbf{x}}_{\mathrm{op}})+\mathbf{H}(\mathbf{x}-{\mathbf{x}}_{\mathrm{op}})$

其中 $\mathbf{H}=\frac{\partial \mathbf{e}}{\partial \mathbf{x}}$ 是所有残差的雅可比（堆叠成矩阵）。

目标函数变为： $J(\mathbf{x})\approx \frac{1}{2}\Vert {\mathbf{W}}^{-1/2}({\mathbf{e}}_{\mathrm{op}}+\mathbf{H}\delta \mathbf{x}){\Vert }^2$

正规方程： $({\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H})\delta {\mathbf{x}}^{\ast }=-{\mathbf{H}}^T{\mathbf{W}}^{-1}{\mathbf{e}}_{\mathrm{op}}$

这就是每次 GN 迭代要解的线性系统。

---

4.4 粒子滤波器：用样本代替公式 §

🎯 直觉：用一群”粒子”追踪可能的轨迹 §

想象你要跟踪一架被云层遮挡的飞机：

• 卡尔曼滤波：假设飞机位置是高斯分布，用一个椭圆表示
• 粒子滤波：扔出 1000 个虚拟飞机，每个按真实动力学飞行
  • 观测到的飞机附近的粒子”存活”
  • 远离观测的粒子”死亡”
  • 重采样：在存活粒子周围生成新粒子

关键优势：不需要知道分布的形状——用样本近似任意分布。

---

🔑 概念层：SIR 粒子滤波器 §

三步骤循环：

1. 传播（Propagate）

每个粒子按（可能非线性的）运动模型独立传播： ${\mathbf{x}}_k^{(i)}\sim p({\mathbf{x}}_k|{\mathbf{x}}_{k-1}^{(i)},{\mathbf{v}}_k)$

这相当于”模拟”系统的可能演化

2. 权重更新（Weight Update）

根据观测计算每个粒子的似然： $w_k^{(i)}\propto p({\mathbf{y}}_k|{\mathbf{x}}_k^{(i)})$

与观测”一致”的粒子获得高权重

3. 重采样（Resample）

按权重随机选择粒子，复制高权重粒子，丢弃低权重粒子。

防止”粒子退化”（少数粒子垄断所有权重）

估计输出： ${\hat{\mathbf{x}}}_k={\sum }_i{w}_k^{(i)}{\mathbf{x}}_k^{(i)}$

---

🧮 数值示例：用粒子滤波定位 §

场景：机器人在走廊里，有一维位置 $x$，观测是到两端的距离。

问题：对称性——距离左端 2 米和距离右端 2 米给出相同观测！

EKF 失败：假设单峰高斯，卡在其中一个解。

粒子滤波：维持双峰分布，两个可能位置都有粒子群。当获得足够观测后，错误位置的粒子自然死亡。

可视化：

时刻 1：  [粒子群A]         [粒子群B]      （双峰）
时刻 2：  [粒子群A]         [粒子群B稀疏]   （B开始死亡）
时刻 3：  [粒子群A密集]     [粒子群B消失]   （收敛到A）

---

📐 推导层：蒙特卡洛积分原理 §

展开：为什么粒子能近似任意分布

核心定理：大数定律

对于任意函数 $f(\mathbf{x})$： $\mathbb{E}[f(\mathbf{x})]=\int f(\mathbf{x})p(\mathbf{x})d\mathbf{x}\approx \frac{1}{N}{\sum }_{i=1}^{N}f({\mathbf{x}}^{(i)})$

其中 ${\mathbf{x}}^{(i)}\sim p(\mathbf{x})$。

粒子滤波的问题：我们如何从 $p({\mathbf{x}}_k|{\mathbf{y}}_{0:k})$ 采样？

答案：重要性采样。从提议分布 $q(\mathbf{x})$ 采样，然后加权： $w^{(i)}\propto \frac{p({\mathbf{x}}^{(i)})}{q({\mathbf{x}}^{(i)})}$

在 SIR 滤波器中，$q$ 选择为转移概率 $p({\mathbf{x}}_k|{\mathbf{x}}_{k-1})$，这使得权重更新简化为观测似然。

---

4.5 拉普拉斯近似：用高斯近似非高斯后验 §

🎯 直觉：在最优点附近拟合一个钟形曲线 §

即使后验不是高斯的，如果我们只关心”最可能的位置”和”附近的不确定性”，可以用高斯近似。

方法：

1. 找到 MAP 估计（最优点）
2. 在最优点附近二阶泰勒展开目标函数
3. 这等价于用高斯近似后验，协方差由 Hessian 矩阵决定

---

🔑 概念层：协方差估计 §

后验近似： $p(\mathbf{x}|\mathbf{y})\approx \mathcal{N}({\mathbf{x}}_{\mathrm{MAP}},{\mathbf{\Lambda }}^{-1})$

其中 $\mathbf{\Lambda }={\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H}$ 是信息矩阵（Hessian）。

实际意义：

• 优化不仅给出”最佳猜测”
• 还给出”猜测的不确定性”
• 这是许多 SLAM 系统的核心

---

本章自检清单 §

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

• EKF 和线性卡尔曼滤波的本质区别是什么？（雅可比 vs 常数矩阵）
• 什么时候 EKF 会失效？（强非线性 + 大步长）
• 粒子滤波如何避免分布假设？（用样本近似）
• 批量 MAP 与 EKF 的本质区别？（迭代优化 vs 单步线性化）

如果以上都清楚，你掌握了非线性估计的核心策略！

---

延伸阅读与代码 §

Python 实现：

import numpy as np
 
def ekf_predict(x, P, f, F, Q):
    """
    EKF 预测步骤
    f: 非线性运动函数
    F: 雅可比矩阵
    """
    x_pred = f(x)
    P_pred = F @ P @ F.T + Q
    return x_pred, P_pred
 
def ekf_update(x_pred, P_pred, y, h, H, R):
    """
    EKF 校正步骤
    h: 非线性观测函数
    H: 观测雅可比
    """
    S = H @ P_pred @ H.T + R
    K = P_pred @ H.T @ np.linalg.inv(S)
    x = x_pred + K @ (y - h(x_pred))
    P = (np.eye(len(x)) - K @ H) @ P_pred
    return x, P
 
 
def particle_filter_propagate(particles, f, Q):
    """粒子传播"""
    N = len(particles)
    for i in range(N):
        particles[i] = f(particles[i]) + np.random.multivariate_normal(np.zeros(len(Q)), Q)
    return particles
 
def particle_filter_update(particles, weights, y, likelihood):
    """权重更新与重采样（系统重采样）"""
    N = len(particles)
    for i in range(N):
        weights[i] *= likelihood(y, particles[i])
    weights /= sum(weights)
 
    # 系统重采样（比简单随机重采样更高效）
    indices = systematic_resample(weights)
    particles = particles[indices]
    weights = np.ones(N) / N
    return particles, weights
 
def systematic_resample(weights):
    """系统重采样：O(N) 复杂度，方差更小"""
    N = len(weights)
    cumsum = np.cumsum(weights)
    u = (np.arange(N) + np.random.uniform()) / N
    return np.searchsorted(cumsum, u)

---

下一章将讨论如何处理估计中的非理想情况（偏差、离群值等）。