本文是《PX4 EKF2 技术详解》系列的第三篇。建议先阅读第一篇：架构与状态空间和第二篇：误差状态动力学，了解 EKF2 的延迟 buffer 架构和协方差传播原理。
前两篇建立了 EKF2 的宏观架构和微观数学基础。本篇以**外部视觉（External Vision, EV）**为具体例子，深入 EKF2 的融合触发机制，回答一个核心问题：
EKF2 的状态更新到底由谁驱动？是 IMU，还是外部传感器？
这个问题的答案决定了你对 EKF2 时间线的理解方式，也是区分 EKF2 与 LPE 本质差异的关键。
1. 为什么选外部视觉作为例子
PX4 EKF2 支持十余种传感器融合源：GNSS、气压计、磁力计、光流、激光雷达、外部视觉......外部视觉（如 VICON、OptiTrack、视觉 SLAM）是理解延迟融合机制的最佳教具，原因有三：
- 延迟明显：EV 数据从摄像头采集到传输到飞控，典型延迟 20~100ms，比 GPS（100~200ms）短但比 IMU（亚毫秒）长，延迟效应清晰可见。
- 多维度输出：EV 可以同时提供位置、速度、姿态（或仅 yaw）、高度，一个传感器就能触发多种融合路径。
- 坐标系复杂：EV 可能在 NED、FRD、BODY 等不同坐标系下输出，EKF2 需要做多层坐标转换，展示了融合框架的通用性。
2. EKF2 主循环：IMU 驱动的时间机器
2.1 核心问题：谁驱动滤波器？
先给出结论，再证明：
EKF2 的滤波主循环由 IMU 驱动，不是由外部传感器驱动。
看 ekf.cpp 的核心循环：
cpp
bool Ekf::update()
{
if (!_imu_updated) {
return false; // 没有新 IMU 数据，什么都不做
}
_imu_updated = false;
// 1. 取最旧的 IMU 样本（延迟时间 horizon）
const imuSample imu_sample_delayed = _imu_buffer.get_oldest();
// 2. 预测：协方差 + 状态
predictCovariance(imu_sample_delayed);
predictState(imu_sample_delayed);
// 3. 融合：检查各传感器是否有与当前 IMU 时间戳对齐的数据
controlFusionModes(imu_sample_delayed);
// 4. Output Predictor 对齐
_output_predictor.correctOutputStates(
imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _gpos,
_state.gyro_bias, _state.accel_bias);
return true;
}关键事实：
| 驱动源 | 行为 | 频率 |
|---|---|---|
| IMU | 触发整个 update() 循环 |
100 Hz（降采样后） |
| EV/GPS/Baro 等 | 被动等待，被 controlFusionModes() 查询 |
各自原始频率，但融合发生在 IMU 时间线上 |
外部传感器的数据到达后，先进入各自的 ring buffer 等待。EKF2 不会立即处理它们------只有当 IMU 驱动的新一轮 update() 到来时，EKF2 才用当前 IMU 的时间戳去各传感器 buffer 中查找对齐数据。
2.2 为什么是 IMU 驱动？
如果把 EKF2 比作一列火车：
- IMU 是铁轨：提供连续的动力学约束，没有 IMU，状态轨迹就断了。
- 外部传感器是车站：提供不定期的校正信号，但火车不能等车站来才开动------火车按自己的时刻表（IMU 频率）运行，路过车站时（时间戳对齐）才上下客（融合）。
那为什么不反过来用传感器事件驱动？传感器事件驱动（如 LPE 的方式）并不会导致状态跳跃------IMU 持续到来时做 predict，状态本身就是连续推进的。EKF2 选择 IMU 定时驱动的真正原因是三个工程问题：
问题 1：时间对齐困难
传感器有延迟且各不相同。GPS 测量的是 200ms 前的位置，EV 是 50ms 前的姿态。事件驱动模式下，传感器到达时直接用"当前状态"做 correct------但当前状态和测量值差了几十到几百毫秒，创新 z − h ( x current ) z - h(x_{\text{current}}) z−h(xcurrent) 是时间错配的伪误差。
LPE 用 _xDelay 查历史状态来缓解，但只是近似（用当前 P 算增益，不是历史 P）。EKF2 的 IMU 定时驱动 + buffer 体系，把 predict 和 correct 严格锁在同一延迟时间戳上，从根本上消除了错配。
问题 2：计算调度复杂
事件驱动下，GPS 10Hz、EV 30Hz、Baro 20Hz......到达时刻由外部硬件/网络决定，可能突发集中。CPU 需要中断式处理，250Hz IMU + 突发传感器，实时性难以保证。EKF2 的 100Hz 固定周期像时钟节拍，计算量可预测，调度简单可控。
问题 3：多传感器顺序更新的耦合一致性
事件驱动模式下，GPS correct 更新了位置和速度，改变了全协方差矩阵；然后 Mag correct 更新姿态，但用的是已经被 GPS "污染"过的 P。这不是联合最优更新，而是顺序近似，24 维交叉耦合的误差会累积。
EKF2 的 IMU 定时驱动在同一时间 horizon 上顺序处理多个传感器，至少保证了它们在同一次 predict 之后的协方差结构下被修正------中间没有额外的 IMU predict 步叠加新的过程噪声，误差可控。
2.3 与 LPE 的本质区别
| 维度 | LPE | EKF2 |
|---|---|---|
| 驱动方式 | 传感器事件驱动 | IMU 定时驱动 |
| 主循环触发 | IMU 回调 → predict；传感器回调 → correct | IMU 回调 → update()（predict + correct 合一） |
| 时间对齐 | 历史状态算创新，当前协方差做修正（近似） | 严格 buffer 对齐：在延迟时间戳上做完整 predict + correct |
| 融合粒度 | 传感器到了就更新 | 传感器被动等待 IMU 时间戳触发 |
| 延迟处理 | _xDelay 保存状态历史 |
IMU + 观测双 buffer 体系 |
LPE 中，GPS 来了就做 GPS correct，Baro 来了就做 Baro correct------每个传感器是"主动敲门"的。EKF2 中，所有传感器"被动等待"，由 IMU 统一调度。
3. FIFO Buffer 体系：延迟对齐的基础设施
EKF2 的延迟融合不是魔法，是靠三级 buffer 体系支撑的工程实现。
3.1 IMU Buffer
cpp
// estimator_interface.cpp:543
_imu_buffer_length = math::max(2, (int)ceilf(_params.ekf2_delay_max / filter_update_period_ms));ekf2_delay_max：最大传感器延迟，默认 110 msfilter_update_period_ms = ekf2_predict_us / 1000，ekf2_predict_us默认 10000 μs（10 ms = 100 Hz）_imu_buffer_length = max(2, ceil(110 / 10)) = 11
IMU buffer 保存最近 11 个降采样后的 IMU 样本，覆盖约 110 ms 的时间窗口。
vehicle_imu 输出的整合 IMU 数据（通常 250 Hz，每个样本已包含 4ms 的 delta angle / delta velocity）进入 EKF2 后，经 ImuDownSampler 进一步累加整合至目标周期（默认 10ms），然后压入 _imu_buffer：
cpp
// estimator_interface.cpp:107
_imu_buffer.push(imu_down_sampled);3.2 观测传感器 Buffer
每个传感器有自己的 buffer。以 EV 为例：
cpp
// estimator_interface.h:365
extVisionSample _ev_sample_prev {};EV buffer 在 setExtVisionData() 中接收数据：
cpp
// estimator_interface.cpp:362
void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
{
if (!_initialised) return;
_ext_vision_buffer->push(evdata); // EV 数据进 buffer
_time_last_ext_vision_buffer_push = _time_latest_us;
}观测 buffer 的长度：
cpp
// estimator_interface.cpp:548-549
const float ekf_delay_ms = _params.ekf2_delay_max * 1.5f; // 考虑 50% jitter
_obs_buffer_length = roundf(ekf_delay_ms / filter_update_period_ms);ekf_delay_ms = 110 * 1.5 = 165 ms_obs_buffer_length = round(165 / 10) = 16 ~ 17
且受限于 IMU buffer：
cpp
_obs_buffer_length = math::min(_obs_buffer_length, _imu_buffer_length);3.3 Output Predictor Buffer
Output Predictor 也分配相同长度的 buffer：
cpp
// estimator_interface.cpp:556
_output_predictor.allocate(_imu_buffer_length); // 同样 11 个样本3.4 三级 Buffer 的关系
时间轴 →
vehicle_imu (250Hz) → [EKF2 内部降采样] → IMU buffer (100Hz, 11个样本) ──┐
│
EV 数据 (30Hz) → EV buffer (16个样本) ──→ pop_first_older_than ──┤──→ EKF2 融合
│
GPS 数据 (10Hz) → GPS buffer ──→ pop_first_older_than ───────────┤
│
Output Predictor buffer (11个样本) ←──────────────────────────────┘核心机制：
- IMU buffer 定义了 EKF2 的时间主轴线 。每次
update()取最旧 IMU，推进一个时间步。 - 各观测 buffer 保存传感器历史数据，EKF2 用
pop_first_older_than(imu_time)精确查找与当前 IMU 时间戳对齐的数据。 - Output Predictor buffer 保存高频 INS 积分历史，用于 EKF2 延迟状态修正后向当前时刻传播。
4. 外部视觉融合全流程
4.1 EV 数据进入系统
EV 数据通过 MAVLink 或 uORB 进入 setExtVisionData()：
cpp
extVisionSample ev_sample;
ev_sample.time_us = timestamp; // EV 采集时刻的时间戳
ev_sample.pos = position_ned; // NED 位置
ev_sample.quat = orientation; // 姿态四元数
ev_sample.vel = velocity; // NED 速度（可选）
ev_sample.pos_frame = LOCAL_FRAME_NED;
ev_sample.vel_frame = LOCAL_FRAME_NED;数据被 push 进 _ext_vision_buffer，等待 EKF2 取用。
4.2 融合触发：pop_first_older_than
在 controlFusionModes() 中：
cpp
// control.cpp:144
controlExternalVisionFusion(imu_delayed);进入 EV 控制逻辑：
cpp
// ev_control.cpp:50
extVisionSample ev_sample;
if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(imu_sample.time_us, &ev_sample)) {
// 找到了与当前 IMU 时间戳对齐的 EV 数据！
// 触发融合...
}关键 ：pop_first_older_than(imu_time) 不是取 buffer 中最旧的数据，而是取第一个时间戳早于 imu_time 的数据。这保证了 EV 数据的时间戳 ≤ IMU 时间戳，即 EV 测量发生在当前 EKF2 融合时刻或之前。
如果没有找到对齐数据（if 条件不满足），本轮 IMU 周期不做任何 EV 融合，只做纯 IMU 预测。
4.3 多维度融合路径
一旦 EV 数据被取出，EKF2 根据 EV 提供的信息类型，触发多条独立的融合路径：
cpp
// ev_control.cpp:67-94
controlEvYawFusion(imu_sample, ev_sample, ...); // Yaw 融合
controlEvVelFusion(ev_vel, ...); // 速度融合（3D）
controlEvPosFusion(imu_sample, ev_sample, ...); // 水平位置融合（2D）
controlEvHeightFusion(imu_sample, ev_sample, ...); // 高度融合（1D）每条路径独立计算创新、创新协方差、卡尔曼增益，然后调用 measurementUpdate() 更新状态。
注意：同一帧 EV 数据可能同时触发 4 条路径，每条路径更新后的状态被下一条路径使用。这是 EKF2 顺序融合（sequential update）的设计------虽然不是严格的联合更新，但在工程上可接受。
4.4 创新计算示例（EV 位置）
以 EV 水平位置融合为例：
预测测量：
z ˉ EV,pos = h ( x ˉ ) = p ˉ x y − R NED EV ⋅ p offset \bar{\mathbf{z}}{\text{EV,pos}} = h(\bar{\mathbf{x}}) = \bar{\mathbf{p}}{xy} - \mathbf{R}{\text{NED}}^{\text{EV}} \cdot \mathbf{p}{\text{offset}} zˉEV,pos=h(xˉ)=pˉxy−RNEDEV⋅poffset
其中 p ˉ x y \bar{\mathbf{p}}{xy} pˉxy 是 EKF2 当前的水平位置， p offset \mathbf{p}{\text{offset}} poffset 是 EV 传感器相对于 IMU 的安装偏移。
实际测量：
z EV,pos = ev_sample.pos ( 0 : 1 ) \mathbf{z}_{\text{EV,pos}} = \text{ev\_sample.pos}(0:1) zEV,pos=ev_sample.pos(0:1)
创新：
r = z − z ˉ \mathbf{r} = \mathbf{z} - \bar{\mathbf{z}} r=z−zˉ
创新协方差：
S = H P H T + R \mathbf{S} = \mathbf{H} \mathbf{P} \mathbf{H}^T + \mathbf{R} S=HPHT+R
其中 H \mathbf{H} H 是位置测量对误差状态的雅可比（仅 pos.x 和 pos.y 位置非零）， R \mathbf{R} R 是 EV 位置观测噪声协方差。
以 EV 四元数融合为例：
EV 提供的 ev_sample.quat 是完整姿态四元数，但 EKF2 的 controlEvYawFusion 只提取其中的 yaw 分量做一维融合------因为 roll/pitch 已由 IMU 重力观测很好约束，yaw 才是外部视觉最增值的信息。
预测测量（从标称姿态提取 yaw）：
z ˉ EV,yaw = h ( x ˉ ) = getEulerYaw ( q ˉ nominal ) \bar{z}{\text{EV,yaw}} = h(\bar{\mathbf{x}}) = \text{getEulerYaw}(\bar{\mathbf{q}}{\text{nominal}}) zˉEV,yaw=h(xˉ)=getEulerYaw(qˉnominal)
实际测量（从 EV 四元数提取 yaw）：
z EV,yaw = getEulerYaw ( q EV ) z_{\text{EV,yaw}} = \text{getEulerYaw}(\mathbf{q}_{\text{EV}}) zEV,yaw=getEulerYaw(qEV)
创新：
r = wrap_pi ( z ˉ EV,yaw − z EV,yaw ) r = \text{wrap\pi}\left(\bar{z}{\text{EV,yaw}} - z_{\text{EV,yaw}}\right) r=wrap_pi(zˉEV,yaw−zEV,yaw)
wrap_pi 保证角度差落在 ( − π , π ] (-\pi, \pi] (−π,π] 区间，避免 2 π 2\pi 2π 跳变。
创新协方差：
S = H YAW P H YAW T + R yaw S = H_{\text{YAW}} \mathbf{P} H_{\text{YAW}}^T + R_{\text{yaw}} S=HYAWPHYAWT+Ryaw
其中 H YAW H_{\text{YAW}} HYAW 是 24 维行向量，由 SymForce 自动生成。在小角度近似下，四元数误差的 z 分量直接对应 yaw 角误差，因此 H YAW H_{\text{YAW}} HYAW 仅在**误差状态第 2 个分量（yaw 角误差）**处为 1，其余为 0：
cpp
// compute_yaw_innov_var_and_h.h (SymForce 生成)
_innov_var = P(2, 2) + R;
_h.setZero();
_h(2, 0) = 1; // 仅 yaw 角误差有贡献这意味着 yaw 融合的传播路径极其清晰：EV 的 yaw 观测只修正姿态的 yaw 分量，不会直接影响位置、速度或偏置------这些耦合由协方差矩阵 P P P 的交叉项间接传递。
那 EV 四元数中的 roll/pitch 信息是不是浪费了？
严格来说，没有直接进姿态估计 。EKF2 没有
controlEvRollPitchFusion，因为 IMU 的重力观测对 roll/pitch 已经是强约束（静止时重力方向直接决定 roll/pitch），精度通常足够。融合完整四元数还会带来冗余约束（4 参数 3 自由度）和观测精度不匹配的问题。但 roll/pitch 的信息间接被用到了------EV 的姿态不确定度会影响位置和速度观测的最小方差：
cpp// ev_pos_control.cpp:104-109 // increase minimum variance to include EV orientation variance // TODO: do this properly const float orientation_var_max = ev_sample.orientation_var.max(); pos_cov(i, i) = math::max(pos_cov(i, i), orientation_var_max);当 EV 姿态抖动大时（如视觉里程计跟踪丢失），位置/速度观测会被自动降权。源码里的
TODO: do this properly说明开发者也意识到这里可以更精细------比如未来可能实现独立的 EV roll/pitch 融合路径。
4.5 卡尔曼更新：measurementUpdate
cpp
// ekf_helper.cpp:1089
bool Ekf::measurementUpdate(VectorState &K, const VectorState &H, const float R, const float innovation)
{
// 1. 清除被抑制状态的增益
clearInhibitedStateKalmanGains(K);
// 2. 状态更新
_state.vector() += K * innovation;
// 3. 协方差更新（约瑟夫稳定形式）
P = (I - K * H) * P * (I - K * H)^T + K * R * K^T;
return true;
}约瑟夫形式 保证了即使数值误差导致 P P P 失去正定性，更新后的 P P P 仍然是半正定的。
4.6 延迟后验状态的形成
所有上述操作------predict、innovation 计算、measurementUpdate------都在同一个延迟时间戳上完成：
当前真实时间：t = 500 ms
EKF2 取 IMU buffer 最旧：t = 400 ms（延迟 100 ms）
↓
predictState(imu@400ms) → 状态推进到 t=400ms
controlFusionModes(imu@400ms)
→ pop_first_older_than(400ms) → EV@395ms
→ innovation@395ms
→ measurementUpdate@395ms
↓
状态更新后的 _state 是 t≈400ms 的最优估计（后验）
↓
correctOutputStates(time_delayed=400ms, _state)
→ 把 _state 与 Output Predictor buffer 中 t=400ms 的历史状态对齐
→ 修正整个历史轨迹这就是延迟后验状态：它不是"当前时刻"的状态，而是"100ms 前"的状态------但这个状态经过了所有传感器的融合，是信息最丰富的最优估计。
5. 融合触发机制详解
5.1 什么时候触发融合？
不是每次 update() 都触发融合。
以 EV（30 Hz）和 EKF2（100 Hz）为例：
| EKF2 周期 | IMU 时间戳 | EV buffer 状态 | 是否触发 EV 融合 |
|---|---|---|---|
| 1 | 0 ms | EV@0ms 存在 | ✅ |
| 2 | 10 ms | EV@0ms 已处理，EV@33ms 还没到 | ❌ |
| 3 | 20 ms | EV@33ms 还没到 | ❌ |
| 4 | 30 ms | EV@33ms 存在 | ✅ |
| 5 | 40 ms | 无新 EV | ❌ |
平均而言，100 Hz 的 EKF2 每 3~4 个周期才会遇到一次新的 EV 数据。其他周期只做纯 IMU 预测。
5.2 多传感器协同
同一帧 IMU 可能同时触发多个传感器：
cpp
// controlFusion.cpp 示意
void Ekf::controlFusionModes(const imuSample &imu_delayed)
{
controlMagFusion(imu_delayed); // 磁力计
controlGnssFusion(imu_delayed); // GPS
controlBaroFusion(imu_delayed); // 气压计
controlExternalVisionFusion(imu_delayed); // 外部视觉
controlOpticalFlowFusion(imu_delayed); // 光流
// ...
}如果当前 IMU 时间戳（400ms）同时命中了：
- GPS@395ms
- EV@398ms
- Baro@400ms
EKF2 会顺序执行三个传感器的融合：
- 先用 GPS 更新状态
- 用更新后的状态计算 EV 的创新
- 再用更新后的状态计算 Baro 的创新
这种顺序更新不是理论上最优的联合更新（需要同时处理所有测量的联合似然），但计算量小得多，且 EKF2 的高频（100 Hz）使得顺序误差被快速修正。
5.3 融合门限与故障检测
每条融合路径都有创新门限检验（innovation gating）：
cpp
if (innovation^T * S^{-1} * innovation < gate_threshold) {
// 创新在预期范围内，执行融合
measurementUpdate(K, H, R, innovation);
} else {
// 创新异常，拒绝融合（可能传感器故障）
_sensor_fault = true;
}EV 各路径的 gate_threshold 默认值不同：yaw 为 2.6（EKF2_HDG_GATE），速度为 3.0（EKF2_EVV_GATE），位置为 5.0（EKF2_EVP_GATE）。如果 EV 数据跳变（如视觉 SLAM 跟踪丢失），创新会超出门限，EKF2 自动拒绝该帧数据，进入纯 IMU 预测模式（dead reckoning）。
6. 延迟对齐的精度问题
6.1 Output Predictor 的时间戳对齐
Output Predictor 的 correctOutputStates() 目前用 get_oldest() 取最旧状态做对齐，源码中有 TODO 指出理想情况下应改用 pop_first_older_than(time_delayed_us) 做精确时间匹配。具体实现细节将在后续文章中展开。
6.2 EV 的时间戳对齐精度
EV buffer 用的是精确的 pop_first_older_than(imu_time)：
cpp
_ext_vision_buffer->pop_first_older_than(imu_sample.time_us, &ev_sample)这保证了 EV 数据的时间戳 ≤ IMU 时间戳，不会用"未来"的测量去修正"过去"的状态。但如果 EV 的延迟大于 ekf2_delay_max，数据会被 buffer 丢弃，导致融合缺失。
7. 小结
EKF2 的融合哲学：
IMU 是火车头，拉着整列状态在时间轴上匀速前进。传感器是沿途车站，火车路过时才上下客。
关键认知：
- IMU 驱动 ：
update()只在有新 IMU 数据时执行，传感器被动等待对齐。 - Buffer 体系 ：IMU buffer（~11个样本）定义延迟 horizon，观测 buffer（~16个样本）保存传感器历史，
pop_first_older_than做精确时间对齐。 - 延迟后验：所有融合在延迟时间戳上完成，输出的是"过去"的最优估计，通过 Output Predictor 传播到当前。
- 顺序融合：多传感器在同一 IMU 周期内顺序更新，每条路径独立做创新检验和故障隔离。
与 LPE 的本质差异：
LPE 是"传感器敲门 → 开门迎客"的事件驱动模式。EKF2 是"火车到站 → 统一上下客"的 IMU 定时驱动模式。前者简单直接但时间对齐不严格，后者复杂但保证了所有传感器在同一时间 horizon 上的一致性------这正是 EKF2 能处理 24 维交叉耦合状态的前提。
附录：EV 融合的源码路径
| 功能 | 文件 | 关键函数 |
|---|---|---|
| EV 数据入口 | estimator_interface.cpp |
setExtVisionData() |
| EV 融合总控 | aid_sources/external_vision/ev_control.cpp |
controlExternalVisionFusion() |
| EV 位置融合 | aid_sources/external_vision/ev_pos_control.cpp |
controlEvPosFusion() |
| EV 速度融合 | aid_sources/external_vision/ev_vel_control.cpp |
controlEvVelFusion() |
| EV 航向融合 | aid_sources/external_vision/ev_yaw_control.cpp |
controlEvYawFusion() |
| EV 高度融合 | aid_sources/external_vision/ev_height_control.cpp |
controlEvHeightFusion() |
| 卡尔曼更新 | ekf_helper.cpp |
measurementUpdate() |
| 主循环 | ekf.cpp |
update() |
| 融合调度 | control.cpp |
controlFusionModes() |
关于我们：
灵智实验室（LingzhiLab）成立于2020年，核心团队源自西北工业大学，由一群深耕无人系统、自动控制与机器人技术的青年工程师与科研人员组成。我们始终秉持"开放、协同、智能、可靠"的理念，致力于推动无人智能体在复杂环境下的自主感知、决策与控制能力。
实验室聚焦于基于开源飞控（如PX4）与ROS 2的深度融合，构建高可靠、模块化、可扩展的无人系统软件架构。依托扎实的工程实践与学术背景，灵智实验室积极参与开源社区建设，助力科研教育与产业落地。