3.3 状态估计：卡尔曼滤波家族
3.3 状态估计：卡尔曼滤波家族
本节摘要：传感器读数是带方差的猜，运动模型也是带方差的猜——状态估计就是把两类猜按可信度加权出更好的猜。本节用一维小车把卡尔曼滤波的五个方程完整推一遍并给代码，再说清扩展卡尔曼与粒子滤波分别在什么情形接班。
一组数字起步
轮式里程计说小车走到了 5.00 米（它的模型预测），激光雷达说小车在 5.30 米（它的测量）。信谁？朴素的两难在加上方差后变成可以计算的问题：里程计漂移大，方差 σ²=1.0；激光测距准，方差 σ²=0.25。按最优加权，融合估计是 (5.00/1.0 + 5.30/0.25) / (1/1.0 + 1/0.25) = 5.24 米。**离准的那边近，但没全信**——这个"按方差倒数的加权平均"正是卡尔曼滤波更新步的全部灵魂，剩下的只是把它推广到多维、动态、连续的场景。
预测与更新：五个方程
卡尔曼滤波把"融合"拆成交替的两步。状态向量 x、协方差 P、过程噪声 Q、测量噪声 R：
预测步（用运动模型外推，不确定性变大）：
x̂ = F·x + B·u （状态沿模型走一步） P̂ = F·P·Fᵀ + Q （协方差也走一步，并加上过程噪声）
更新步（用测量修正，不确定性变小）：
y = z − H·x̂ （新息：测量与预测的差） K = P̂·Hᵀ·(H·P̂·Hᵀ + R)⁻¹ （卡尔曼增益：按方差倒数分配权重） x = x̂ + K·y （修正状态） P = (I − K·H)·P̂ （修正协方差，不确定性收缩）
K 是整个算法的聪明所在：测量方差 R 越小（测量可信），K 越接近 1，结果越信测量；预测协方差 P̂ 越小（模型自信），K 越接近 0，结果越信预测。开头那组数字就是这个公式的标量特例。而 P̂ = F·P·Fᵀ + Q 一步里不确定性单调增大、更新步单调收缩，一涨一缩之间，滤波器对"我现在的估计有多可信"永远保持诚实——这个协方差还会被第 4 章规划器拿去算安全裕度。
图：预测-更新循环中的不确定性涨落
一维滤波器完整代码
用匀速小车跑通五个方程。输入：速度指令 u=1 m/s（含噪声），带噪位置测量，各 100 步：
import numpy as np dt = 0.1 F, H = np.array([[1.0, dt], [0.0, 1.0]]), np.array([[1.0, 0.0]]) Q = np.diag([0.01, 0.05]) # 过程噪声：位置、速度 R = np.array([[0.25]]) # 测量噪声方差（开头例子的 0.25） x = np.array([[0.0], [0.0]]) # 初始状态：位置 0，速度 0 P = np.eye(2) rng = np.random.default_rng(42) true_v, true_pos, errs = 1.0, 0.0, [] for _ in range(100): true_pos += true_v * dt # ---- 预测 ---- x = F @ x P = F @ P @ F.T + Q # ---- 更新 ---- z = true_pos + rng.normal(0, 0.5) # 测量：标准差 0.5 m y = z - (H @ x)[0, 0] K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R) x = x + K * y P = (np.eye(2) - K @ H) @ P errs.append(abs(x[0, 0] - true_pos)) print("融合估计平均误差: %.3f m" % np.mean(errs)) # 典型输出: 融合估计平均误差: 0.17 m 左右 # 对照：只用测量（0.5m 噪声）平均误差约 0.4m；只用模型预测会漂到米级
0.17 米这个数字就是"两类猜加权融合"的实物证明：好于任何单一来源，且协方差 P 全程同步记录着这份可信度。
家族成员与接班条件
标准卡尔曼只对线性高斯世界严格成立。机器人世界两处不配合，各有一位接班人：
扩展卡尔曼滤波（EKF）：运动或观测方程非线性（里程计的圆弧运动学、相机的投影）。做法是把非线性函数在当前点做一阶泰勒展开，用雅可比矩阵替代 F 与 H。代价是展开点选错会发散——快速转弯、强机动场景要小心；无人车与无人机的经典方案（EKF-SLAM、EKF 导航）服役多年。
粒子滤波：分布严重非高斯（多峰——机器人可能在两扇一模一样的门前）、或方程根本写不光滑时接班。用几千个带权重的粒子直接采样表示分布，重采样让"活得好"的粒子繁衍。代价是算力：粒子数从几百到几十万，方差随维度指数膨胀，高维状态不友好。
互补滤波：无人机姿态估计的廉价明星——不估计协方差，直接频域分工：陀螺仪高频可信、加速度计低频可信，高通加低通一拼。公式一行，效果在四旋翼上好到惊人，是"不追求最优、追求够用"的代表。
⚠️ 常见坑：噪声参数 Q、R 拍脑袋给。它们是滤波器的人格：R 给小了滤波器追着测量噪声抖，Q 给小了滤波器迟钝跟不上机动。工程做法是给真实日志回放调参，或用残差一致性检验（新息的实际方差应与理论值一致）自动校准。
本节要点
- 状态估计 = 按方差倒数加权两类信息源：模型预测与传感器测量，五个方程完整描述。
- 预测步不确定性涨（+Q），更新步缩（×K），协方差 P 是滤波器对自身可信度的诚实记录。
- 线性高斯用 KF，非线性用 EKF（雅可比展开），非高斯多峰用粒子滤波，姿态融合可用互补滤波。
- Q 与 R 是必须用日志回放校准的关键参数，拍脑袋的滤波器要么抖要么钝。
多速率滤波：工程里的常态
教材里的滤波器假设所有测量同频到达，现实是 IMU 100 赫兹、雷达 10 赫兹、编码器 500 赫兹。多速率滤波的标准处理：每个测量到达就做一次更新（各自的 R 与 H），没有测量时只做预测——滤波器天然支持"测量随到随融"。真正的坑在时间戳：乱序到达的测量（网络抖动导致旧测量晚到）必须重排或丢弃，否则更新次序错乱、协方差失真。工程实现里维护一个小的重排缓冲（按时间戳排序后逐个更新），是"能用"与"偶尔发疯"的分水岭。
滤波器的自检：新息一致性
滤波器对不对，看新息（测量与预测之差）。健康状态：新息的统计方差应与理论方差（H·P̂·Hᵀ + R）一致——滤波器预测的"不确定范围"与实际的偏差范围吻合。实际方差长期大于理论值，说明 Q 或 R 给小了（滤波器过度自信）；新息均值长期不为零，说明模型有偏（比如里程计比例因子没标好）。把新息检验做成在线监控，滤波器就从"黑盒"变成"带自检的仪表"，大量定位异常能在恶化前被发现。这套检验与第 5 章的时序看板配合，是感知运维的基础设施。
问题：UKF 什么时候值得上
扩展卡尔曼在强非线性下的雅可比展开会失真（快速转弯、大角度机动），无迹卡尔曼（UKF）换一种思路：不解析求导，而是按确定性规则采一组 sigma 点穿过非线性函数，用变换后的点均值协方差近似分布。精度高于 EKF，计算量约为其两到三倍。工程判断：姿态估计这类强非线性状态（四元数在 EKF 里要特殊处理）值得 UKF；普通二维导航 EKF 足够，不必为百分之几的精度改善引入实现复杂度。选型次序建议：先 KF，不够再 EKF，仍不够再 UKF，粒子滤波留给非高斯场景——升级要有数据支撑（新息检验的劣化证据），而不是直觉。