第 8 章滤波与融合¶

第 7 章「姿态与坐标表达」把姿态表示备好，本章解决"带噪的多路测量怎么合成一个可信状态估计"。主线是最小实现（python numpy 手写）走通加权融合、互补滤波、卡尔曼滤波（Kalman filter, KF）与扩展 KF（EKF），第三方库（filterpy）只作对照。

重点在两处：过程/观测/静态三协方差的调参判据，与数值卫生（对称性、正定性、Joseph 形式）。估计的统计模型侧写作依据取自 Barfoot《State Estimation for Robotics》与 Thrun《Probabilistic Robotics》。运行环境 python 3.11 + numpy/scipy + filterpy 2.0.0。

从加权平均走到 EKF 的递进，以及三协方差的作用点：

flowchart LR
    z["多路带噪测量"] --> w["加权融合<br/>按 1/σ²"]
    w --> comp["互补滤波<br/>低通 + 高通"]
    comp --> kf["KF：预测 / 更新"]
    kf --> ekf["EKF：非线性航位推算 + 雅可比"]
    cov["三协方差：过程 Q / 观测 R / 静态"] -.-> kf
    ekf --> out["状态估计 x̂"]

1. 加权融合与贝叶斯起点¶

最朴素的融合：两路对同一量的独立高斯估计 \((x_1,\sigma_1^2),(x_2,\sigma_2^2)\)，最优（最小方差无偏）合成是逆方差加权——权重 \(w_i=1/\sigma_i^2\)：

\[ x_f=\frac{w_1x_1+w_2x_2}{w_1+w_2},\qquad \sigma_f^{-2}=w_1+w_2 . \]

取 \((2.0,\,0.5),(2.4,\,0.2)\)：

融合值=2.3448 融合std=0.1857（比两路都小） 权重比 w2:w1=6.25（方差小的占大权）
贝叶斯核对：先验 N(2.0,0.25) × 似然 N(2.4,0.04) -> 后验 N(2.3448,0.0345)

融合值偏向更可信的 \(x_2\)（权重大 6.25 倍），融合不确定度反而小于任一路——这是"1/s2 加权"的全部直觉，也是贝叶斯后验=先验×似然的标量特例。KF/EKF 无非是把这条逐维、逐时刻、带状态转移地做下去。（“1/σ² 加权”中的 σ² 为方差）

2. 互补滤波：低通 + 高通的几何图像¶

两路信息频谱互补时用互补滤波：陀螺给角速度（积分得高频好、低频漂移），加计给倾角（低频准、高频噪）。一步递推

\[ \hat\theta_k=(1-\alpha)(\hat\theta_{k-1}+\dot\theta_k\,dt)+\alpha\,\theta^{\text{accel}}_k , \]

对陀螺路径是低通、对加计路径是高通，两者相加恒等于 1（互补）。单摆真角度 + 两路噪声（陀螺测速 σ=0.5、加计测角 σ=3.0），扫 \(\alpha\)：

alpha=0.01: RMSE=0.252°   alpha=0.05: RMSE=0.405°   alpha=0.20: RMSE=0.967°
纯陀螺积分 RMSE=0.693°（漂移）   纯加计 RMSE=3.038°（噪声）

\(\alpha\) 是带宽旋钮：小 \(\alpha\)=更信陀螺积分（噪声 0.25°最好，因本例加计太脏），大 \(\alpha\)=更快拉向加计但吞进它的高频噪声。任何 \(\alpha\) 都优于单独一路（0.25–0.97° 全 < 陀螺 0.69° 或加计 3.04° 之一），这就是融合的净赚。互补滤波是 KF 在标量、固定增益下的退化像——KF 的妙处正在于那个 \(\alpha\)（增益）由协方差递推自动算出来。

3. KF 推表：恒速模型走一遍¶

KF 交替做两件事——预测（模型外推，协方差涨）与更新（测量拉回，协方差缩）。恒速状态 \(x=[p,v]^\top\)、只测位置（\(H=[1,0]\)）：

\[ \begin{aligned} &\text{预测: } x^{-}=Fx,\quad P^{-}=FPF^\top+Q\\ &\text{更新: } y=z-Hx^{-},\ S=HPH^\top+R,\ K=PH^\top S^{-1},\ x=x^{-}+Ky,\ P=(I-KH)P \end{aligned} \]

跟踪 \(v=1\) 的真位置（\(\Delta t=0.01\)、\(R=0.5\)、小 \(Q\)）：

KF 末端位置=5.220（真=4.990） 速度=1.213（真=1.0） 全程位置 RMSE=0.180
filterpy 对照末端 x=[5.22, 1.213]（与最小实现差=1.33e-15）

手写实现与 filterpy 数值差 1e−15，说明推表实现无误；速度估计 1.21 偏高是 \(Q_v\) 给了速度自由度、滤波器允许速度漂移去贴合噪声位置的正常现象（调参见「三协方差调参」一节）。增益 \(K\) 就是「互补滤波」那个 \(\alpha\) 的多维、时变版：\(P\) 大（模型没底）→ \(K\) 大更信测量，\(R\) 大（测量脏）→ \(K\) 小更信模型。

预测与更新交替，数值卫生挂在更新侧：

flowchart LR
    st["当前 x, P"] --> pred["预测：x⁻ = Fx<br/>P⁻ = FPFᵀ + Q"]
    pred --> upd["更新：K = P⁻Hᵀ(HP⁻Hᵀ+R)⁻¹<br/>x = x⁻ + K(z − Hx⁻)"]
    upd --> nxt["下一拍"] --> pred
    jose["对称化 / Joseph 形式<br/>保持 P 正定"] -.-> upd

4. EKF：非线性航位推算最小闭环¶

机器人运动学与观测多是非线性。EKF（extended Kalman filter）在每次预测/更新处绕当前估计一阶线性化，用 Jacobian 代替线性矩阵，递推结构与 KF 同。取非线性观测 \(h(p)=p+0.2p^2\)（Jacobian \(\partial h/\partial p=1+0.4p\)）、恒速传播、\(\Delta t=0.01\)：

EKF 末端位置=4.903（真=4.990） 速度=0.922（真=1.0）——绕当前估线性化仍收敛
对比：把非线性观测 z 直接当位置喂线性 KF（忽略 h），系统偏差 h(p)-p=0.2p² 在末端达 4.98

这正是 IMU 航位推算的形态：状态传播用运动模型（含姿态积分，见第 7 章「姿态与坐标表达」\(\dot q=\frac12 q\otimes(0,\omega)\)），观测方程非线性（测距、测角、里程计位移），EKF 在估计点线性化。偏差对比那行说明为何不能用线性 KF 硬扛非线性观测——忽略 \(h\) 会留下随位置平方增长的系统偏差。EKF 的通病也在此埋下：线性化点一旦偏、或非线性太强（\(h\) 二阶项主导），一阶近似失效、协方差低估、滤波发散；更强的非线性该换 UKF（见「UKF 与粒子滤波」一节）。

5. 过程/观测/静态三协方差调参¶

滤波器只有三个可调统计量：过程协方差 \(Q\)（模型多不可信）、观测协方差 \(R\)（测量多脏）、初始协方差 \(P_0\)（起点多没底；收敛后被前两者主导，属"静态"一次性旋钮）。扫 \(Q\)（固定 \(R\)）：

Q×  1: 稳态位置增益≈0.052 跟随 RMSE=0.626
Q× 10: 稳态位置增益≈0.140 跟随 RMSE=0.582
Q×100: 稳态位置增益≈0.365 跟随 RMSE=0.456

\(Q/R\) 比是唯一的实质自由度（同乘常数不改增益轨迹）：\(Q\) 大＝承认模型易错＝增益高＝快跟但抖（跟随误差随 \(Q\) 降、噪声抑制变差）；\(Q\) 小＝信模型＝增益低＝平滑但滞后。调参判据回到第 4 章「频率域与鲁棒性」的频域直觉——\(Q/R\) 定的是滤波器的等效带宽。IMU 器件侧的噪声密度、零偏不稳定性怎么折成 \(Q,R\) 的数值，属器件误差源，归 hardware-note（预留的 MEMS/惯性传感章节），本章只取它们作为观测模型的输入。

6. 数值卫生：对称性、正定性、Joseph 形式¶

协方差 \(P\) 数学上永远对称正定，数值上会漂。朴素更新 \(P=(I-KH)P\) 只在 \(K\) 恰为最优 \(PH^\top S^{-1}\) 时保持对称；用一般/定增益时对称性丢失，几十步后 \(P\) 甚至失正定（开根、求逆即崩）。同一递推改用 Joseph 形式

\[ P=(I-KH)P(I-KH)^\top+KRK^\top, \]

对任意 \(K\) 都显式保对称、且右端两项分别半正定故保正定。定增益观测器 100 步对照：

100 步: 朴素最大不对称=3.776e+00   Joseph 最大不对称=4.441e-16
朴素对称化后最小特征值=-9.861e-01（已负→失正定）   Joseph 最小特征值=1.061e-01（始终≥0）

朴素版不对称涨到 3.8、最小特征值转负（协方差非法）；Joseph 对称度守在 1e−16、特征值恒 ≥0。工程规矩三条：每步强制对称化 \(P\leftarrow(P+P^\top)/2\)；用 Joseph 形式（或至少在其后对称化）；监测 \(P\) 特征值不转负。多路同步长航时的滤波器发散，十有八九是这里省了一步。

7. UKF 与粒子滤波：非线性强度分档¶

7.1 分档判据与实证对照¶

EKF 之外按非线性强度还有两档，点名 + 最小实证（filterpy 2.0.0 同参数对照）：

无迹 KF（unscented Kalman filter, UKF）：用确定性采样的一组 sigma 点直接传均值与协方差，免算 Jacobian、抓到二阶以上的非线性，成本仍是协方差级。适合"中等强非线性、状态维不高"（纯测距、带姿态的航位推算常见）。
粒子滤波（particle filter）：蒙特卡洛样本 + 重采样，扛强非线性、非高斯、多峰后验，成本随粒子数与状态维数暴涨（维数灾难）。适合低维、后验形态复杂（如初始定位的多峰、离散跳变）。

分档判据用三 beacon 纯测距 + 恒速模型的同一套仿真落锤（初值偏 3.2 m、测距 σ=0.5 m、60 拍）：

三 beacon 测距，初值偏 3.2 m:  EKF 末端误差=0.177 m   UKF(α=0.3)=0.176 m —— 同量级，EKF 已够用
双 beacon，初值跨镜像对称线:   EKF 收错峰 3.08 m    UKF 卡错解 1.72 m  —— 单高斯无解
1D 测距 d=|x|（±1.2 双峰）:   PF 两团质心 -1.204/1.201（质量 0.50/0.50），
                              加第二 beacon 后收敛 1.203±0.094；EKF 单高斯押错边（误差 2.39）

三条读法：其一，测距观测配恒速模型、温和初值下 EKF 与 UKF 同量级——EKF 的线性化已够，UKF 的实际增益在免 Jacobian（观测函数只给函数值、不给导数）。其二，初值跨过测距几何的镜像对称线是多峰问题，EKF/UKF 的单高斯假设无解；且 EKF 收到错误镜像解后协方差照样缩小、看似收敛，不报错——「收敛」与「收对」是两回事。其三，粒子滤波两团共存、可被后续观测消歧，代价是维数：本例 1 维 2000 粒子，6 维同精度需百万级。

UKF 另有一条工程边界：α 是 sigma 点外扩旋钮，α=1 时协方差失正定直接发散、α=0.3 稳定——免 Jacobian 不等于免调参。手写 sigma 点递推与 filterpy 同参数全程最大差 1.7e−14；其 update 从预测后的 \((x,P)\) 重新生成 sigma 点（忠于 Wan & van der Merwe 原始算法），与「同一批点复用到观测步」的写法数值不可混用。

分档判据一句话：线性 → KF；可微且非线性温和 → EKF（省，但要正确 Jacobian）；非线性强但维数低、想避开 Jacobian → UKF（α 从小往大试）；非高斯/多峰/低维 → 粒子滤波。里程计与观测器怎么把这些滤波器串成定位系统，在第 9 章「里程计与观测器」展开。

7.2 实现工具面：scipy 与 filterpy 的边界¶

三类滤波器都有现成实现，但要分清“拿来用”与“拿来对照”：

scipy.signal：butter/sosfiltfilt 设计零相位滤波（离线平滑、生成参考轨迹、事后分析），savgol_filter 做滑动多项式平滑。实时回路里一阶 IIR 手写更可控——scipy 的 sos 结构适合批处理，不适合每拍几微秒的预算。
scipy.optimize：least_squares 做参数辨识与静态标定（自带信赖域与数值雅可比，比手写 Gauss-Newton 省事），minimize 用于小规模协方差级优化。
scipy.sparse：状态维数大时（几十维以上的 EKF）信息矩阵与 Joseph 形式的稀疏更新——稠密矩阵 \(O(n^3)\) 的每拍开销不可行时，这是唯一出路。
filterpy：KF/EKF/UKF/PF 的参考实现，适合当作数值对照（本节 UKF 实现即与之逐拍对照）与快速原型；卡开发时直接用，上产品前再按自己的状态维与观测模型重写。

判据一句话：离线分析与标定用 scipy；实时回路能用闭式解与一阶 IIR 就不引 scipy（体积、延迟、可预测性都是理由）；滤波器库只当对照基准，不当运行时依赖。

8. 要点¶

融合的内核是逆方差加权（\(\sigma\) 小的占大权、融合方差不减反增），它就是贝叶斯后验=先验×似然的标量特例。
互补滤波=陀螺低通+加计高通、系数和为 1，\(\alpha\) 是带宽旋钮；任何 \(\alpha\) 的 RMSE 都优于单独一路（0.25–0.97° vs 陀螺0.69°/加计3.04°）。
KF 的增益 \(K\) 是 \(\alpha\) 的多维时变版，由 \(P,R\) 自动定；手写与 filterpy 差 1e−15。
EKF 绕当前估计线性化处理非线性观测；忽略 \(h\) 用线性 KF 会留随 \(p^2\) 增长的系统偏差（末端 4.98），EKF 收敛到 4.90。
三协方差里只有 \(Q/R\) 比值是实质自由度：大 \(Q\) 快跟但抖、小 \(Q\) 平滑但滞后；器件噪声折成 \(Q,R\) 归 hardware-note。
数值卫生必做：Joseph 形式对任意增益保对称保正定（朴素版 100 步不对称涨到 3.8、特征值转负），每步 \((P+P^\top)/2\) 并监测特征值。
非线性分档带实证：三 beacon 测距温和初值 EKF≈UKF（末端 0.18 m 同量级）；初值跨镜像 EKF/UKF 单高斯全挂（错峰 3.1/1.7 m）；PF 双峰共存消歧（1.203±0.094）但维数是天花板；UKF α=1 发散、α=0.3 稳。滤波链展开归第 9 章「里程计与观测器」。

参考文献与延伸阅读¶

Kalman, R. E. A New Approach to Linear Filtering and Prediction Problems. Journal of Basic Engineering, 1960. DOI
Julier, S. J., & Uhlmann, J. K. A New Extension of the Kalman Filter to Nonlinear Systems. Proc. SPIE, 1997. DOI
Thrun, S., Burgard, W., & Fox, D. Probabilistic Robotics. MIT Press, 2005.

第 8 章 滤波与融合¶