跳转至

ESKF 验证与踩坑实录

15 态误差状态卡尔曼滤波（ESKF）从"低机动能收敛、一上高机动就炸"到"65.4s 高机动剖面收敛到 1.82°"的全过程复盘。每条坑都带根因与修复，可直接迁移到其它基于 ESKF/EKF 的惯性导航项目。 项目背景：STM32H743VIH6 航姿参考板，3×IMU + 2×磁力计 + 2×气压计 + GNSS，MATLAB 参考（eskf15_ref.m）与固件 C（ins_eskf_15d.c）双实现、逐拍对拍验证。

1. 高机动发散：三个非标准错误叠加，靠一个 quirk 侥幸掩盖

症状

  • 4s 低机动轨迹：收敛良好（att RMS 0.046°）；
  • 用户改轨迹为 65.4s VTOL 剖面（2.5g/3.0g 转弯 + AoA 180° 倒飞）：发散到 51~180°。

低机动能过、高机动就炸——这是"非标准实现 + 侥幸掩盖"类 bug 的典型特征。

根因（三个错误叠加）

  1. 错误的 F 矩阵伪耦合：F(θ,bg)=−R（应为 −I）、F(p,v)=−R（应为 +I）、漏了 F(v,v)、还多出 F(v,p)/F(p,p)/F(p,ba)/F(p,θ) 一堆错项 → 产生错误的 P(θ,p)/P(θ,v) 交叉协方差。高机动时 GNSS 修正位置，错误的交叉项把姿态一步推飞。
  2. 量测雅可比多乘了一个 R：写成了 H = R'·skew(v) = skew(h)·R，正确应为 H(θ) = +skew(h)。
  3. 朴素协方差更新 P=(I−KH)P：数值上会把 P 坍缩到量测噪声地板之下 → 伪收敛。

掩盖机制：一个 reset_att 钩子把姿态协方差块清零到 1e-9。低机动时误差小，清零不暴露问题；高机动一上来，错误交叉项直接引爆。

修复：标准 Solà 右误差 ESKF，F/H/误差注入三者自洽

F(θ,θ)=−[ω]×;  F(θ,bg)=−I;   F(p,v)=+I
F(v,θ)=−R·[â]×; F(v,v)=−[ω]×; F(v,ba)=−R
雅可比 H(θ)=+skew(h)，h=R'·[0;0;−g]（accel）/ R'·m（mag）
协方差：Joseph 稳定式 P=(I−KH)P(I−KH)'+KRK'

机动处理（decoupled gating，关键）：accel 模型 h=R'·[0;0;−g] 只在近水平 1g 成立，banked turn（1.05~1.58g）下系统性错误 → 加速度加硬幅值门限（|‖a‖−g|>0.3 跳过）；磁力计只依赖姿态、不依赖载荷 → 始终参与。曾试过协方差下限 att_floor 反而更糟（attRMS 升到 11~14°），证明 accel 在 1g 倾斜段是"污染"姿态而非修正 → 用门限而非下限。

验证结果

场景 修复前 修复后
4s 低机动 0.046° 0.046°（不变）
65.4s 高机动 发散 51~180° att RMS 1.820°（参考==固件，3 位小数对齐）

2. baro 消融：气压计与姿态块数学上解耦

旧记录"单独加 baro 发散到 ~90°"曾长期误导。用修复后的标准 ESKF 实测：

  • baro 观测方程 h = −p(3)，雅可比 H_b 只在位置行有 −1，姿态块全零——从数学上它就不可能把姿态推飞；
  • 实测五档配置：baro 开/关 attitude 完全一致（4s 0.046°/0.046°，65.4s 1.840°/1.839°）；
  • 去掉 accel 只留 mag+GNSS+baro 反而更优（0.153~0.282°）；
  • 只有缺 GNSS 的配置才漂：mag 雅可比 skew(h_m) 秩=2，绕磁场矢量的姿态自由度不可观，与 baro 无关。

结论：旧结论是 buggy 滤波的症状，不是 baro 之过。教训：历史结论要随根因修复重新验证，不要拿旧结论当现状。

3. float/double 假阴性：等价性判据不能拍脑袋

Phase 2 用 MEX/Simulink 把固件 C 包起来与 MATLAB 参考逐拍对拍，自动判据 pos_cr_max < 1e-3（1mm）报 MISMATCH——其实是假阴性：

  • C 是单精度 float、参考是双精度 double，4000 步积分后浮点舍入缓慢累积（实测 22µm@0.5s → 1.68mm@3.36s，随步数有界增长，非发散）；
  • 1.68mm > 1mm → 误报。

修复：判据随时长缩放——pos_cr_max < 5e-3 × (T/4.0)（T=4s 时即 5mm，约 3× 实测漂移裕量；长轨迹自动放大）。为什么安全：真算法不一致会先崩姿态（几十度）到位置米级，5mm 比真实发散小约 3000 倍。

教训：判据要锚定"数值噪声地板"（实测漂移的固定倍数），而不是拍脑袋的绝对数；且判据应多通道联合（att/vel/euler 仍严卡），放宽某一项不放松整体。

4. MATLAB 封装真实 C 的坑（MEX / S-Function）

  • Level-2 C S-Function：用 ssGetInputPortRealSignal 前必须 ssSetInputPortRequiredContiguous(S,0,1) + 显式 ssSetInputPortDataType(S,0,SS_DOUBLE)/ssSetOutputPortDataType，否则输入读成未初始化垃圾（≈1.2e-311），状态冻结在 init。
  • To-File 块 "Array" 格式是转置布局：ysim 实为 14×N（行 1=时间，行 2:14=输出通道），读时要转置；Simulink 比参考多一个末点，须按 M=min(rows) 对齐。
  • MEX 网关在 MSVC 2019 下 mxGetDoubles 链接失败（LNK2019），须用经典 mxGetPr。
  • matlab -batch 拒绝以 _ 开头的脚本名（解析错误），测试脚本命名别用下划线开头。

5. 商用对标教训：第三方页面必须先核对 datasheet 版本

把 Xsens MTi 1-series 拉进设计讨论做对标时，两个来源数字打架：

指标 北微传感产品页 Xsens 官方 mtidocs
陀螺 in-run 零偏 10 °/h 6 °/h
陀螺噪声密度 0.007 °/s/√Hz 0.003 °/s/√Hz
加速度计噪声 120 µg/√Hz 70 µg/√Hz

原因：北微页给的是 MTi v2.x 代次，官方 v3.x/v5.x 已升级。教训：采信原厂官方文档，第三方/代理页面的数字先核对 datasheet 版本再引用。

6. 真机隐患：磁力计幅值检查的参数不一致

核对固件时发现 eskf15_check_mag(..., 1.05f, 55.0f) 的幅值阈值 str=55µT 与 mag 参考向量 [54.6,-6.7,47.6] 的模（≈72.7µT）不匹配：|72.7−55| > 0.3×55 → 幅值检查恒 false → 真机上磁力计更新会被全部拒绝。SIL 没走 check_mag，所以此隐患在纯仿真里完全不可见，直到搭"真实数据回放"核对固件调用链才暴露。

修正方向二选一：①mag_ned_ref 用实测定标值（使模≈55µT，北京真实总场约 55µT）；②str 改为 norm(mag_ned_ref)。

教训：验证手段覆盖不到的代码路径（本例：真机才执行的磁有效性检查），要在核对调用链时人工兜底。

7. 小结：踩坑的共性

  1. 自洽性：F/雅可比/误差注入三者必须同属一个标准约定（本例：Solà 右误差），混搭必然出隐蔽 bug。
  2. 口径：比较精度/性能前先确认口径（合成真值 vs 实测典型值、单精度 vs 双精度、v2.x vs v3.x）。
  3. 验证覆盖：每层验证只能覆盖"它执行到的代码"，覆盖不到的路径（真机才跑的检查、第三方数据）要单独兜底。
  4. 历史结论要随修复重验：根因修好后，旧结论（如"baro 单独加发散"）要作废重测。

相关：验证矩阵与真实数据回放 · MATLAB ESKF 算法验证（Phase 1） · PC 端 SIL 实战