PX4状态估计技术EKF2详解（五）：EKF2 故障检测、重置与鲁棒性——从单实例到多实例仲裁

EKF2 的精度来自多传感器融合，但它的可靠性来自故障检测与恢复机制 。当 GPS 信号被高楼遮挡、磁力计受电机干扰、IMU 出现数据跳变时，EKF2 如何识别故障、拒绝坏数据、必要时重置状态？更进一步，PX4 v1.11 引入的多实例仲裁（Multi-EKF）让多个 EKF2 实例并行运行、相互备份------但这在低端处理器上是性能与安全的 trade-off。本文深入这些鲁棒性机制的实现细节。

---

1. 为什么需要故障检测

前四篇文章假设了一个理想化模型：传感器始终在线，测量噪声严格服从零均值高斯分布，未考虑数据丢失、非高斯异常和硬件故障。但真实飞行中，传感器随时可能出问题：

故障场景	表现	后果
GPS 多径	位置跳变几十米	位置估计被污染，无人机漂移
磁力计干扰	yaw 突变 90°	航向错乱，位置估计发散
IMU 数据跳变	加速度计瞬间异常	速度/位置积分错误
激光雷达失效	高度数据丢失	高度估计纯靠 IMU，快速漂移
视觉 SLAM 跟踪丢失	位置冻结或跳变	EV 融合引入错误

如果没有故障检测，EKF2 会把这些坏数据当作"真值"融合进去，协方差虽然会膨胀，但状态已经被污染。等污染累积到 innovation gate 触发时，可能已经太晚了。

---

2. Innovation Gate：传感器数据的质量门禁

2.1 Test Ratio 的计算

每次传感器融合前，EKF2 计算创新比率（test ratio），衡量观测值与预测值的偏离程度：

test_ratio=r2(gate⋅σr)2=innovation2gate2⋅innovation_variance \text{test\_ratio} = \frac{r^2}{(\text{gate} \cdot \sigma_r)^2} = \frac{\text{innovation}^2}{\text{gate}^2 \cdot \text{innovation\_variance}} test_ratio=(gate⋅σr)2r2=gate2⋅innovation_varianceinnovation2

• r=z−zˉr = z - \bar{z}r=z−zˉ：创新（观测值减预测值）
• σr=S\sigma_r = \sqrt{S}σr=S ：创新标准差
• gate：门限倍数（参数，控制允许几倍标准差）

判断规则（硬门限）：

• test_ratio < 1：创新在门限内，数据可信，允许融合
• test_ratio ≥\geq≥ 1：创新超出门限，数据被拒绝（innovation_rejected = true）
• test_ratio 为 NaN 或 innovation 为 NaN：同样拒绝

例如 gate = 3.0 时，test_ratio < 1 等价于 ∣innovation∣<3σ|\text{innovation}| < 3\sigma∣innovation∣<3σ；gate = 5.0 时等价于 ∣innovation∣<5σ|\text{innovation}| < 5\sigma∣innovation∣<5σ。gate 越大，允许的正常噪声范围越宽。

2.2 低通滤波：防止瞬时噪声导致的误拒绝

传感器噪声是高频的，如果直接用瞬时 test_ratio 判断，可能导致"正常数据偶尔被拒"。EKF2 对 test_ratio 和 innovation 做一阶低通滤波：

α=ΔtΔt+τ,τ=0.5 s \alpha = \frac{\Delta t}{\Delta t + \tau}, \quad \tau = 0.5 \text{ s} α=Δt+τΔt,τ=0.5 s

test_ratio_filtered←test_ratio_filtered+α⋅(sign(r)⋅test_ratio−test_ratio_filtered) \text{test\_ratio\_filtered} \leftarrow \text{test\_ratio\_filtered} + \alpha \cdot \left( \text{sign}(r) \cdot \text{test\_ratio} - \text{test\_ratio\_filtered} \right) test_ratio_filtered←test_ratio_filtered+α⋅(sign(r)⋅test_ratio−test_ratio_filtered)

滤波后的 test_ratio_filtered 比瞬时 test_ratio 更平滑，减少了噪声导致的误触发。需要注意的是，test_ratio_filtered 是有符号 的（乘了 sign(r)），用于 Selector 仲裁和状态诊断。

当单次 test_ratio > 4（即创新超过 2⋅gate⋅σ2 \cdot \text{gate} \cdot \sigma2⋅gate⋅σ）时，才对滤波值做限制：

static constexpr float kNormalizedInnovationLimit = 2.f;
static constexpr float kTestRatioLimit = sq(kNormalizedInnovationLimit);  // = 4

if (test_ratio > kTestRatioLimit) {
    status.test_ratio_filtered = constrain(test_ratio_filtered, -kTestRatioLimit, kTestRatioLimit);
}

这里的 4 是滤波报告值的防饱和上限 ，不是融合判断门限。融合判断永远是 test_ratio < 1。

2.3 各传感器的 test_ratio

传感器	test_ratio	gate 默认值
速度（GNSS/EV）	vel_test_ratio	3.0（EKF2_EVV_GATE / EKF2_GPS_V_GATE）
水平位置（GNSS/EV）	pos_test_ratio	5.0（EKF2_EVP_GATE）
高度（Baro/GPS/激光雷达）	hgt_test_ratio	根据传感器不同
航向（磁力计/EV/GPS）	hdg_test_ratio	2.6（EKF2_HDG_GATE）
空速	tas_test_ratio	---
离地高度	hagl_test_ratio	---

关键设计：不同传感器的 gate 不同。GPS 位置 gate 设为 5（较宽松），因为 GPS 本身有 2~5 m 的噪声，太紧的门限会导致正常数据被频繁拒绝；磁力计航向 gate 设为 2.6（较严格），因为航向误差会快速扩散到位置估计。

2.4 与 LPE 卡方检验的对比

第 3 篇（LPE 传感器融合）中，LPE 使用卡方检验判断传感器数据是否可信。两种方案在数学形式、统计基础和工程策略上都有显著差异，体现了从 LPE 到 EKF2 的设计演进。

数学形式

方案	统计量	数学表达式
LPE 卡方检验	马氏距离平方	β=rTS−1r\beta = \mathbf{r}^T \mathbf{S}^{-1} \mathbf{r}β=rTS−1r
EKF2 test_ratio	单维标准化创新平方	test_ratio=ri2gate2⋅Sii\text{test\ratio} = \dfrac{r_i^2}{\text{gate}^2 \cdot S{ii}}test_ratio=gate2⋅Siiri2

LPE 的 β\betaβ 是多维 的：S=CPCT+R\mathbf{S} = \mathbf{C}\mathbf{P}\mathbf{C}^T + \mathbf{R}S=CPCT+R 是完整的 ny×nyn_y \times n_yny×ny 新息协方差矩阵，S−1\mathbf{S}^{-1}S−1 同时考虑了各测量分量之间的相关性。例如 GPS 同时测量位置和速度，位置和速度新息之间通过协方差耦合，LPE 的卡方检验能捕捉这种耦合异常。

EKF2 的 test_ratio 是单维 的：由于磁力计、GPS、气压计等传感器在 EKF2 中采用顺序融合 （sequential fusion，一维一维地更新），每次只处理一个创新分量 rir_iri，对应的 SiiS_{ii}Sii 是该分量的方差。多维马氏距离在此框架下不自然适用。

统计基础

方案	分布假设	阈值来源
LPE	β∼χ2(ny)\beta \sim \chi^2(n_y)β∼χ2(ny)	预计算卡方分布临界值表（false alarm = 10−410^{-4}10−4）
EKF2	ri/Sii∼N(0,1)r_i / \sqrt{S_{ii}} \sim \mathcal{N}(0, 1)ri/Sii ∼N(0,1)	工程经验门限（gate = 2~5）

LPE 的卡方检验是严格统计假设检验 ：给定 false alarm probability，查表得到临界值，超过即判定为故障。EKF2 的 gate 则是工程门限 ：gate = 3 对应约 3σ\sigmaσ（99.7% 置信区间），gate = 5 对应约 5σ\sigmaσ。没有严格的 false alarm 控制，但实现简单、计算量小。

故障后的处理策略

这是两者最深刻的差异：

方案	GPS 故障后	其他传感器故障后
LPE	强制继续更新 （return 不执行）	激光雷达等直接 return
EKF2	直接拒绝，进入 dead reckoning	直接拒绝，进入 dead reckoning

LPE 的设计哲学是"坏的绝对参考比没有参考更好 "------GPS 即使被判定为故障，仍强制融合，因为纯 IMU 积分按 t2t^2t2 发散，几十秒内位置完全失控。EKF2 的设计哲学则更为谨慎：一旦 test_ratio 超过门限，立即拒绝该传感器，依赖其他传感器或纯 IMU 预测。这种差异的根本原因在于 EKF2 的传感器冗余更丰富（磁力计、气压计、EV、GPS 等多源辅助），单个传感器被拒止后系统不会立即失稳；而 LPE 的传感器配置相对简单，GPS 往往是唯一的全局位置参考。

低通滤波的差异

EKF2 对 test_ratio 做了一阶低通滤波（时间常数 0.5 s），防止瞬时噪声导致的误拒绝。LPE 没有显式的低通滤波------卡方检验本身对单次采样的判断已经足够鲁邦（阈值极宽，GPS 放宽 100 倍），不需要额外滤波。但这也意味着 LPE 对持续性轻微异常的响应更慢。

还有一个细节：逐维计算 vs 整体拒绝

EKF2 中 GPS 速度有 vx/vy/vz 三个 test_ratio，GPS 位置有 x/y 两个------每个维度分别计算 。但只要任一维度 test_ratio ≥ 1，innovation_rejected 置 true，整个传感器全部维度都被拒止。不是"好维度继续融、坏维度跳过"。

LPE 中 GPS 是 6 维整体算一个卡方统计量 β=rTS−1r\beta = \mathbf{r}^T \mathbf{S}^{-1} \mathbf{r}β=rTS−1r，整体过不过。两种方案最终都是"全融或全不融"，只是数学路径不同：LPE 从多维耦合出发，EKF2 从逐维判断后取逻辑或。

---

3. 传感器超时与降级：从融合到纯预测

3.1 Timeout 检测：检测的是"多久没成功融合过"，不是"多久没收到数据"

注意一个关键细节：time_last_fuse 记录的是上一次成功融合的时间戳，不是"上一次收到数据"的时间戳。这意味着 timeout 触发的场景不止"传感器断连"一种：

场景	数据是否到达	是否触发 timeout
GPS 模块死机、串口断开	❌ 没到	✅
数据持续被 innovation gate 拒绝（test_ratio ≥ 1）	✅ 到了但被拒	✅
数据延迟太大，buffer 找不到同步的 IMU 样本	✅ 到了但滞后	✅
传感器未初始化，控制标志不允许融合	✅ 到了但不融	✅

bool isTimedOut(uint64_t last_fuse_time, uint64_t timeout) {
    return (last_fuse_time + timeout < _time_delayed_us);
}

timeout 后调用 stopFusion()，清除该传感器的控制标志（如 _control_status.flags.gnss_pos = false），EKF2 从此不再尝试融合该传感器，直到重新满足激活条件。这和"单次融合被拒绝"（test_ratio ≥ 1）有本质区别------后者只是"这一帧不融，下一帧再试"，timeout 则是"彻底放弃，进入降级"。

各传感器的超时阈值：

• GPS：约 1~2 秒
• 磁力计：约 1 秒
• 气压计：约 1 秒
• EV：约 1 秒

3.2 Dead Reckoning：纯 IMU 预测

传感器超时后，EKF2 停止该传感器的融合，进入纯 IMU 预测（dead reckoning）模式：

• 状态继续由 IMU 驱动积分（predictState + predictCovariance）
• 协方差持续膨胀（因为没有观测约束）
• 位置/速度精度随时间快速退化

纯 IMU 预测的精度退化速度：

• 姿态 ：陀螺偏置导致漂移，约 1°1°1°/分钟（消费级 MEMS）
• 速度 ：加速度计偏置导致漂移，约 0.10.10.1 m/s² 积分后 666 m/s/分钟
• 位置：速度误差二次积分，约几十米/分钟

这意味着 dead reckoning 只能维持几十秒的可用精度，之后必须依赖 GPS 恢复或其他传感器重新融合。

3.3 协方差膨胀：告诉下游"我不确定了"

超时期间，EKF2 不仅停止融合，还会主动增大协方差，向下游传递"不确定性增加"的信号：

// 纯预测期间，协方差按过程噪声持续增长
P = A * P * A^T + Q

Output Predictor 看到协方差增大后，互补滤波的增益会自适应调整（tau 不变，但 EKF2 本身的精度下降会导致 Output Predictor 的跟踪误差增大）。控制器也可以通过 estimator_status 中的 pos_test_ratio、vel_test_ratio 判断当前估计质量。

---

4. 状态重置：当估计器"走丢"时

状态重置不是由"单次创新超门限"触发的------那是 test_ratio ≥ 1 的职责，只拒绝当前帧，下一帧继续尝试。重置触发的是另一个条件：距离上次成功融合已经太久了。

bool isTimedOut(uint64_t last_fuse_time, uint64_t timeout) {
    return (last_fuse_time + timeout < _time_delayed_us);
}

last_fuse_time 是上一次成功融合的时间戳。只要融合持续被拒绝（test_ratio ≥ 1）、数据没到、或数据滞后无法同步，这个时间戳就不更新，timeout 就会累积。

各状态重置的超时阈值：

重置类型	超时参数	时间	触发后行为
GNSS 位置/速度	reset_timeout_max	7 秒	允许则 reset 到测量值；不允许则 stop + gnss_fault = true
磁力计	reset_timeout_max	7 秒	reset yaw 或停止磁力计融合
高度（气压计/激光雷达/GPS）	hgt_fusion_timeout_max	5 秒	isHeightResetRequired() 触发 reset
Bad Accel 高度异常	bad_acc_reset_delay_us	0.5 秒	垂直加速度持续异常，立即 reset

reset 后先尝试把状态直接设为观测值；如果 reset 不允许（如已有其他传感器可用），则直接 stopFusion()，不再尝试恢复。

---

4.1 高度重置

当高度传感器（气压计/GPS/激光雷达）与当前高度估计严重不符时：

resetAltitudeTo(new_altitude, new_altitude_variance);

• 直接修改 _state.pos(2) 为观测高度
• 重置高度方向的协方差 P(9,9)（第 9 个误差状态，对应 z 位置）
• 同时重置 Output Predictor 的垂直 buffer（resetAltitudeTo）
• 记录 _information_events.flags.reset_hgt_to_*

4.2 位置重置

GPS 长时间丢失后重新捕获，或视觉 SLAM 重新定位成功：

resetHorizontalPositionTo(new_position, new_position_variance);

• 修改 _state.pos(0:1)
• 重置水平位置协方差 P(7:8, 7:8)
• 重置 Output Predictor 的水平位置 buffer
• 如果位置变化大于阈值（如 5 m），还会触发位置跳变事件（position jump event），通知控制器和导航模块

4.3 速度重置

GPS 速度突变（如固定翼进入/退出滑翔）、视觉里程计尺度恢复：

resetVelocityTo(new_velocity, new_velocity_variance);

• 修改 _state.vel
• 重置速度协方差 P(4:6, 4:6)
• 重置 Output Predictor 的速度 buffer

4.4 姿态/Yaw 重置

磁力计 yaw 与当前估计严重冲突（如室内起飞后出室外，磁环境剧变）：

resetQuatStateYaw(new_yaw, new_yaw_variance);

• 提取当前 roll/pitch，只替换 yaw 分量
• 更新四元数 _state.quat_nominal
• 重置航向协方差 P(2,2)
• 关键 ：yaw 重置后，水平速度会被旋转补偿------因为速度是在 NED 坐标系下估计的，yaw 变化意味着机体坐标系与 NED 的对应关系变了

const float yaw_diff = wrap_pi(new_yaw - old_yaw);
resetHorizontalVelocityToMatchYaw(yaw_diff);

4.5 全状态重置：最后的手段

当协方差矩阵出现数值问题（如负特征值、非正定）时，EKF2 会触发协方差重置：

initialiseCovariance();

这相当于"初始化重来"------把协方差矩阵恢复到对角阵，对角元素设为较大的初始方差。状态向量本身不重置，但增益会暂时增大，让后续观测快速"拉"回正确值。

---

5. 多实例仲裁（Multi-EKF）：从单点故障到冗余备份

前面的故障检测和重置都是单实例内的自救机制。但如果 IMU 本身故障（数据跳变、陀螺零漂），或者 EKF2 参数配置不当导致该实例始终不健康，单实例就无能为力了。多实例仲裁正是为了解决这个问题。

5.1 为什么需要多实例

PX4 支持的飞行平台从 FPV 小机到大型固定翼，传感器配置差异巨大：

平台	IMU	磁力计	GPS	典型场景
小型穿越机	1×	无/内置	可选	室内飞行
多旋翼航拍机	1~2×	内置+外置	1×	户外航拍
工业巡检机	2~3×	外置高精度	RTK	长距离任务
固定翼	1~2×	外置	1×+RTK	高速巡航

不同 IMU 有不同的噪声特性，不同磁力计有不同的磁干扰敏感度。如果只有一个 EKF2 实例，IMU 故障 = 整个状态估计崩溃。

多实例的核心思想 ：让多个 EKF2 实例并行运行，每个实例使用不同的 IMU（和可选的不同磁力计），然后由一个仲裁器（Selector）选择最健康的实例输出。

5.2 实例创建逻辑：IMU × MAG

多实例的数量由两个参数决定：

• EKF2_MULTI_IMU：使用几个 IMU（1 ~ MAX_NUM_IMUS，通常为 3）
• EKF2_MULTI_MAG：每个 IMU 搭配几个磁力计（1 ~ MAX_NUM_MAGS，通常为 4）

实例总数 = min⁡(IMU 数×MAG 数,EKF2_MAX_INSTANCES)\min(\text{IMU 数} \times \text{MAG 数}, \text{EKF2\_MAX\_INSTANCES})min(IMU 数×MAG 数,EKF2_MAX_INSTANCES)：

#if CONSTRAINED_MEMORY
# define EKF2_MAX_INSTANCES 2
#else
# define EKF2_MAX_INSTANCES 9
#endif

硬件平台	内存限制	EKF2_MAX_INSTANCES
STM32F4（旧版 Pixhawk）	192 KB SRAM	2
STM32F7（Pixhawk 4）	512 KB SRAM	9
STM32H7（Pixhawk 6X）	1 MB SRAM	9

实例的传感器组合：

实例 0: IMU0 + MAG0
实例 1: IMU0 + MAG1
实例 2: IMU1 + MAG0
实例 3: IMU1 + MAG1
...

每个实例是独立的 EKF2 对象 ，有自己的 _state、_P、buffer、参数。它们并行运行，互不干扰。

5.3 仲裁评分：test_ratio + relative_test_ratio

Selector 每秒评估一次所有实例的健康度。核心指标是综合 test ratio：

combined_test_ratio = max(0.5 * (vel_test_ratio + pos_test_ratio), hgt_test_ratio)

• 速度和位置的 test_ratio 取平均后乘以 0.5
• 与高度的 test_ratio 取最大值
• 这个设计保证：任何一维（水平位置、速度、高度）严重异常都会拉高综合评分

健康判定：

healthy = (filter_fault_flags == 0) && (combined_test_ratio > 0)

• filter_fault_flags != 0：EKF2 内部检测到数值错误（如协方差非正定、除零）
• combined_test_ratio <= 0：test_ratio 无效（通常是初始化阶段）

相对误差评分（relative_test_ratio）：

Selector 不仅看单个实例的绝对 test_ratio，还跟踪与当前主实例的相对差异：

error_delta = instance[i].combined_test_ratio - instance[selected].combined_test_ratio
instance[i].relative_test_ratio += error_delta

• 如果备用实例的 test_ratio 长期低于主实例，relative_test_ratio 会累积为负值
• 只有当 relative_test_ratio <= -threshold 且持续 10 秒未当选时，才会触发"性能切换"
• threshold = max(EKF2_SEL_ERR_RED, 0.05)，默认 0.1

这个设计避免了频繁切换------不会因为某帧噪声导致备用实例暂时更好就立即切换。

5.4 切换策略：故障切换 vs 性能切换

Selector 的切换逻辑分三层：

第一层：主实例故障 → 立即切换

if (!instance[selected].healthy) {
    // 优先选择使用不同 IMU 的健康实例
    if (!SelectInstance(best_ekf_different_imu)) {
        // 否则选择 test_ratio 最好的健康实例
        SelectInstance(best_ekf);
    }
}

• 如果主实例不健康（timeout、filter fault、IMU fault），立即切换
• 优先选择使用不同 IMU 的实例------因为当前 IMU 可能硬件故障

第二层：备用实例明显更好 → 延迟切换

if (lower_error_available
    && (time_since_last_change > 10_s)
    || (warning && time_since_last_warning > 1_s)) {
    SelectInstance(best_ekf_alternate);
}

• 即使主实例仍然健康，如果备用实例的 relative_test_ratio 显著更低，也会切换
• 但要求：上次切换至少 10 秒前，避免震荡

第三层：用户手动指定 → 强制切换

if (_request_instance.load() != INVALID_INSTANCE) {
    SelectInstance(user_requested_instance);
}

• 通过 MAVLink 或地面站手动指定使用某个实例

5.5 切换时的 Reset 传播

实例切换不是简单的"换个数据源"------不同实例的状态可能有偏移（如位置差了几米、yaw 差了几度）。Selector 在切换时做了reset 传播：

姿态切换：

if (instance_change) {
    _quat_reset_counter++;
    _delta_q_reset = Quatf(new_attitude.q) * Quatf(last_attitude.q).inverse();
}

• 计算新旧实例的姿态差 Δq\Delta\mathbf{q}Δq
• 累加到全局 reset counter
• 下游控制器通过 quat_reset_counter 和 delta_q_reset 检测到姿态跳变，可做平滑处理

位置切换：

if (instance_change) {
    _xy_reset_counter++;
    _delta_xy_reset = Vector2f(new_pos.x, new_pos.y) - Vector2f(last_pos.x, last_pos.y);
}

• 计算 XY 位置差
• 控制器收到 xy_reset_counter 增加后，知道位置有跳变，可重置位置环积分

为什么需要 reset 传播？

控制器（如 mc_pos_control）内部有位置误差积分。如果 EKF2 实例切换导致位置跳变 5 米，而控制器不知道这个跳变是"估计器切换"而非"真实移动"，它会认为无人机突然偏离目标 5 米，从而输出巨大的修正力矩。reset 传播告诉控制器："这是估计器重置，不是你的错，请清零积分"。

5.6 低端处理器的 CPU Load 问题（重点）

多实例仲裁不是免费的午餐。每增加一个 EKF2 实例，CPU 负载大致增加：

组件	单实例开销
EKF2 内核（predict + fuse）	1525% STM32F7 CPU
Output Predictor	~5%（与实例数无关，Selector 只选一个输出）
EKF2Selector 仲裁	12%

实例数与总负载的关系：

实例数	STM32F7 估算负载	可行性
1	20~25%	✅ 宽裕
2	40~50%	✅ 正常
3	60~75%	⚠️ 紧张
4	80~100%	❌ 危险
9	>200%	❌ 不可能

注意： Selector 只输出一个 实例的结果给控制器，但所有实例都在后台运行。这意味着 9 实例配置下，9 个 EKF2 都在做 predict + fuse，Selector 只是选其中一个输出。

为什么 EKF2 这么耗 CPU？

• 24×24 协方差矩阵传播：每次 predict 约 243≈1382424^3 \approx 13824243≈13824 次浮点运算
• 100 Hz 更新频率：每秒 138 万次浮点运算
• 多个传感器融合：GPS、磁力计、气压计、EV 等，每条路径都有矩阵乘法

PX4 的应对策略：

1. 内存受限板子限制为 2 实例 ：CONSTRAINED_MEMORY 宏将 EKF2_MAX_INSTANCES 限制为 2
2. 动态实例分配：只在起飞前分配实例，起飞后不再创建新实例
3. Work Queue 分离：每个实例运行在独立的 work queue 上，利用多核（如果有）
4. SITL 默认关闭多实例 ：Disable multi-ekf for SITL------仿真环境不需要硬件冗余

实际调参建议：

平台	推荐实例数	理由
F4 旧板（192KB RAM）	1	内存不够，跑一个都紧张
F7 标准板（512KB RAM）	2	主 IMU + 备用 IMU，最实用的冗余
H7 高端板（1MB+ RAM）	3~4	主 IMU + 备用 IMU，各配一个磁力计
9 实例	不推荐	除非你有非常强力的处理器和明确需求

关键认知 ：多实例的冗余价值在IMU 故障 时最大。如果你的飞控只有 1 个 IMU，开再多 EKF2 实例也没用（它们用同一个脏数据）。多实例的首要前提是硬件冗余------先有多个 IMU，再谈多实例仲裁。

---

6. IMU 故障检测：两层架构

IMU 一致性检查不是 EKF2 独有的 ，而是横跨 sensors 模块和 ekf2 模块的两层防线。

6.1 第一层：VotedSensorsUpdate（sensors 模块，数据进入 EKF2 之前）

VotedSensorsUpdate 同时做两件事：

1. 传感器投票 ：从多个 IMU 中选出最佳一个，输出 sensor_combined
2. 不一致性计算 ：把各 IMU 与均值的差异发布到 sensors_status_imu

// voted_sensors_update.cpp
void calcGyroInconsistency() {
    gyro_mean = mean(gyro_all[i]);
    _gyro_diff[i] = 0.95f * _gyro_diff[i] + 0.05f * (gyro_all[i] - gyro_mean);
}

• 计算各陀螺仪与均值的差异，一阶低通滤波（时间常数约 0.2 s）
• 发布 gyro_inconsistency_rad_s[i] 到 sensors_status_imu
• 加速度计同理，发布 accel_inconsistency_m_s_s[i]

这一层在进入 EKF2 之前 就完成了------所有 EKF2 实例收到的已经是 sensor_combined 中选定的 IMU 数据，以及 sensors_status_imu 中的不一致性指标。

6.2 第二层：EKF2Selector（ekf2 模块）

Selector 不直接读 IMU 原始输出 ，它订阅的是 sensors_status_imu，用第一层已经算好的不一致性指标，再做累积积分判断长期故障：

// EKF2Selector.cpp
if (_sensors_status_imu.updated()) {
    _accumulated_gyro_error[i] += (gyro_inconsistency - angle_rate_threshold) * dt;
    if (_accumulated_gyro_error[i] > angle_threshold) {
        _gyro_fault_detected = true;
        faulty_gyro_id = sensors_status_imu.gyro_device_ids[largest_error_index];
    }
}

判定规则：

• 3+ 个 IMU：误差最大的那个被判定为故障，可精确定位
• 2 个 IMU：只能判定"存在故障"，无法确定是哪个
• 1 个 IMU：无法检测故障

为什么需要两层？

• VotedSensorsUpdate 只做瞬时差异检测，时间尺度在毫秒级
• EKF2Selector 做累积积分，时间尺度在秒级------防止瞬时机体振动导致的误报

6.3 IMU 故障 → 实例级联失效

当 Selector 检测到某个 IMU 故障时：

if (gyro_fault_detected && (instance[i].gyro_device_id == faulty_gyro_id)) {
    instance[i].healthy = false;  // 立即标记该实例不健康
}

• 使用该故障 IMU 的所有 EKF2 实例立即被标记为不健康
• Selector 优先切换到使用不同 IMU 的实例
• 如果所有实例都使用该 IMU（如只有 1 个 IMU），则系统无冗余可用，只能继续用故障数据

---

7. 参数与调参

7.1 多实例参数

参数	含义	默认值	建议
EKF2_MULTI_IMU	IMU 实例数	0（自动）	根据实际 IMU 数设置
EKF2_MULTI_MAG	磁力计实例数	0（自动）	有外置磁计时可设 2
EKF2_SEL_ERR_RED	切换误差阈值	0.1	越小切换越敏感

7.2 Innovation Gate 参数

参数	含义	默认值	调参建议
EKF2_GPS_V_GATE	GPS 速度 gate	3.0	GPS 噪声大时放宽
EKF2_EVP_GATE	EV 位置 gate	5.0	视觉抖动大时放宽
EKF2_HDG_GATE	航向 gate	2.6	磁干扰环境放宽
EKF2_RNG_GATE	激光雷达 gate	5.0	地形起伏大时放宽

7.3 Timeout 参数

参数	含义	默认值
EKF2_NOAID_TIMEOUT	无辅助传感器超时	3 s
EKF2_GPS_DELAY	GPS 延迟	110 ms
EKF2_EV_DELAY	EV 延迟	50 ms

---

8. 小结

EKF2 的鲁棒性来自三层防线：

层级	机制	作用范围	响应速度
第一层	Innovation Gate（test_ratio）	单实例内，单次融合	实时（每帧）
第二层	传感器超时检测	单实例内，传感器级	秒级
第三层	多实例仲裁（Selector）	跨实例，系统级	秒级（含 10s 延迟防震荡）

关键认知：

1. Innovation gate 是第一道防线，但门限不能设得太紧------否则会拒绝正常噪声，导致融合缺失、协方差膨胀
2. 状态重置是最后的自救，代价是位置/姿态跳变，可能惊动控制器
3. 多实例仲裁是真正的冗余，但前提是硬件冗余（多个 IMU）。低端处理器上，2 实例是性价比最高的配置
4. Selector 的 relative_test_ratio 设计很精妙：避免了频繁切换，只在备用实例长期显著优于主实例时才切换

关于我们：

灵智实验室（LingzhiLab）成立于2020年，核心团队源自西北工业大学，由一群深耕无人系统、自动控制与机器人技术的青年工程师与科研人员组成。我们始终秉持"开放、协同、智能、可靠"的理念，致力于推动无人智能体在复杂环境下的自主感知、决策与控制能力。

实验室聚焦于基于开源飞控（如PX4）与ROS 2的深度融合，构建高可靠、模块化、可扩展的无人系统软件架构。依托扎实的工程实践与学术背景，灵智实验室积极参与开源社区建设，助力科研教育与产业落地。