PX4状态估计技术EKF2详解（一）：EKF2 开篇——从分离到统一

EKF2 开篇：从分离到统一，PX4 状态估计的范式转移

EKF2 是 PX4 当前默认且推荐的状态估计器。attitude_estimator_q 和 local_position_estimator 虽仍存在于源码库中（可作为非默认选项编译），但已不再活跃维护。PX4 的演进方向清晰：从 attitude_estimator_q + local_position_estimator 的分离架构，转向 EKF2 的统一架构。本文解释这个范式转移的动机，以及 EKF2 的核心设计如何在精度、延迟和计算资源之间取得平衡。

1. 为什么学 EKF2

本系列前面的文章已经覆盖了两个独立的估计器：

attitude_estimator_q：基于 Mahony 互补滤波的姿态估计器。它用陀螺仪积分跟踪姿态，加速度计修正 roll/pitch，磁力计修正 yaw，输出四元数供其他模块使用。
local_position_estimator ：基于 10 维卡尔曼滤波器的位置估计器（实现上近似为线性时变 KF，官方文档称其为 EKF）。状态为 [x, y, z, vx, vy, vz, bx, by, bz, tz]，接收 attitude_estimator_q 的姿态做坐标变换，融合 GPS、气压计、激光雷达等测量更新速度和位置。

这两个模块各自独立运行，通过 uORB 消息传递数据。这种"姿态归姿态、位置归位置"的分离架构在简单场景下可以工作，但存在三个根本性问题：

姿态误差和位置误差相互耦合：加速度计偏置会导致 attitude_estimator_q 的姿态漂移，而错误的姿态会把重力分量投影到水平方向，进一步污染 LPE 的速度和位置估计。分离模式下，attitude_estimator_q 不知道 LPE 的反馈，LPE 也无法修正 attitude_estimator_q。
观测信息无法跨域流动：GPS 的速度测量可以同时约束姿态（通过比力方程）和位置（通过速度积分），但在分离模式下，GPS 速度只进入 LPE，attitude_estimator_q 看不到。同样，磁力计的航向信息只进入 attitude_estimator_q，LPE 无法利用磁场约束来辅助位置估计。
固定翼的侧滑角约束无法利用：固定翼巡航时，机头方向与飞行方向天然对齐（侧滑角近似为零），这个强约束需要同时知道姿态和速度才能施加。分离模式下，attitude_estimator_q 不知道速度，LPE 不知道准确姿态，两边都无法利用这个约束。

EKF2 通过将姿态和位置放到同一个状态空间内统一解算 ，解决了这些问题。它不是两个模块的简单合并，而是一个误差状态卡尔曼滤波器（Error-State Kalman Filter, ESKF），在数学上允许任意状态分量之间的交叉修正。

但这不意味着 EKF2 是"更精确的 attitude_estimator_q + LPE"。EKF2 的根本差异在于问题建模的维度------它估计的是一个完整的飞行力学系统，而 attitude_estimator_q + LPE 估计的是两个割裂的子系统。这个维度提升带来了能力，也带来了复杂性。本系列的目标就是逐层拆解这种复杂性，理解 EKF2 在每个设计选择上的权衡。

2. PX4 状态估计的演进

2.1 分离式时代：attitude_estimator_q + local_position_estimator

在 PX4 v1.7 之前（约 2017 年），默认的状态估计架构是分离式：

复制代码

IMU ──┬──→ attitude_estimator_q ──→ 姿态（四元数）
      │                                    │
      └──→ local_position_estimator ───────┤
               ↑                           │
           GPS/气压计/激光雷达            用于坐标变换

attitude_estimator_q 基于显式互补滤波器（explicit complementary filter，借鉴了 Mahony 的陀螺/加计融合公式）：

用陀螺仪积分跟踪高频姿态变化
用加速度计（假设测量值以重力为主）修正 roll/pitch 的零漂
用磁力计修正 yaw 的零漂
更新频率 250~1000Hz

local_position_estimator 基于 10 维卡尔曼滤波器（实现上近似为线性时变 KF，官方文档称其为 EKF）：

状态：[x, y, z, vx, vy, vz, bx, by, bz, tz]
输入：IMU 加速度（经过 attitude_estimator_q 的姿态旋转到 NED）
测量：GPS、气压计、激光雷达、光流、视觉
更新频率 80~250Hz

分离模式的问题：

姿态误差单向流动：attitude_estimator_q 的姿态输出是 LPE 的输入，但 LPE 的估计结果无法反向修正 attitude_estimator_q。如果 attitude_estimator_q 的 yaw 有 5° 偏差，LPE 会基于错误的姿态一直累积位置误差，直到 GPS 校正它------但 GPS 校正的是位置，不是姿态。
加速度计偏置的双重身份：在 attitude_estimator_q 中，加速度计用于修正 roll/pitch；在 LPE 中，加速度计用于预测速度。如果加速度计有偏置，两个模块会分别处理，没有统一模型。
信息丢失：GPS 的速度测量包含机体坐标系下的运动信息，理论上可以通过比力方程反推姿态误差。但 LPE 看不到原始 IMU 数据（只看旋转后的加速度），attitude_estimator_q 看不到 GPS 数据，这个信息通道被完全切断。

2.2 统一式时代：EKF2

PX4 从 v1.8 开始将 EKF2 设为默认，后续版本中 attitude_estimator_q 和 local_position_estimator 逐步退出默认编译配置，开发重心完全转向 EKF2。EKF2 的架构是：

复制代码

IMU ──→ EKF2（统一解算）──→ 姿态 + 速度 + 位置 + 偏置 + 磁场 + 风 + 地形
                ↑
        GPS / 气压计 / 磁力计 / 激光雷达 / 光流 / 视觉 / 空速计

EKF2 的原始状态与误差状态

EKF2 使用 ESKF（Error-State Kalman Filter），需要区分两个概念：

原始状态（storage） ：实际存储在内存中的数值，共 25 维：

状态分量	storage 维度	说明
四元数 qqq	4	[q0,q1,q2,q3][q_0, q_1, q_2, q_3][q0,q1,q2,q3]，单位约束 q02+q12+q22+q32=1q_0^2+q_1^2+q_2^2+q_3^2=1q02+q12+q22+q32=1
速度 vvv	3	NED 线速度
位置 ppp	3	NED 位置
陀螺仪偏置 bgb_gbg	3	角速度偏置，单位 rad/s
加速度计偏置 bab_aba	3	加速度偏置，单位 m/s²
地磁场 mIm_ImI	3	NED 地球磁场
机体磁场 mBm_BmB	3	body 硬铁/软铁磁场
风速 vwv_wvw	2	水平风速
地形高度	1	地面高度
合计	25	---

误差状态（tangent） ：卡尔曼滤波实际估计的状态误差，共 24 维：

误差分量	tangent 维度	说明
姿态误差 δθ\delta\boldsymbol{\theta}δθ	3	旋转矢量（小角度近似），替代四元数的 4 维存储
速度误差 δv\delta vδv	3	---
位置误差 δp\delta pδp	3	---
陀螺偏置误差 δbg\delta b_gδbg	3	---
加速度偏置误差 δba\delta b_aδba	3	---
地磁误差 δmI\delta m_IδmI	3	---
机体磁误差 δmB\delta m_BδmB	3	---
风误差 δvw\delta v_wδvw	2	---
地形误差 δhterr\delta h_{terr}δhterr	1	---
合计	24	---

四元数的特殊性 ：原始状态用 4 维存储，但四元数的切空间（SO(3) 的李代数）只有 3 维。ESKF 在 3 维误差空间上做卡尔曼更新，然后映射回 4 维四元数空间。这是 ESKF 的核心------卡尔曼滤波器只操作 24 维误差状态，不涉及 4 维四元数的直接更新。

统一解算的关键优势需要区分直接修正 和间接耦合：

直接修正 ：由测量雅可比矩阵 H\mathbf{H}H 的非零元素决定。如果 h(x)h(\mathbf{x})h(x) 直接依赖某个状态，该状态就会被直接修正。
间接耦合 ：由误差协方差矩阵 P\mathbf{P}P 的非对角项传播。即使 H\mathbf{H}H 对某个状态没有直接偏导，该状态仍可能通过与其他状态的协方差耦合被间接修正。

传感器	测量方程直接依赖的状态（H\mathbf{H}H 非零）	间接耦合的状态（通过 P\mathbf{P}P 传播）
加速度计（比力方程）	姿态（roll/pitch）	速度、位置（姿态误差 →\to→ 重力投影错误 →\to→ 速度污染）
磁力计	姿态（航向）+ 地磁 mIm_ImI + 机体磁 mBm_BmB	几乎无
GPS 速度	水平/垂直速度	姿态（微弱，通过 PqvP_{qv}Pqv 二阶耦合）
气压计	高度 zzz + 地形	垂直速度（通过 PvzP_{vz}Pvz）
激光雷达	高度 zzz + 地形	垂直速度（通过 PvzP_{vz}Pvz）

核心认知 ：统一解算的真正价值不是"任意传感器可以修正任意状态"，而是测量方程可以天然地同时依赖多个状态 （如磁力计 h=RT⋅mI+mBh = R^T \cdot m_I + m_Bh=RT⋅mI+mB 同时依赖姿态和磁场），而协方差耦合只是让这个多状态修正更加平滑。GPS 速度对姿态的间接修正是微弱且次要的，不是 EKF2 姿态估计的主要信息来源。

2.3 固定翼的特殊收益：侧滑角约束

统一解算对固定翼机型有一个独特的好处------侧滑角伪观测。

固定翼在巡航阶段，机头方向与飞行方向天然对齐，侧滑角（sideslip angle，机体 x 轴与来流方向在水平面内的夹角）近似为零。这个约束可以写成观测方程：

β=arctan⁡(vrel,ybodyvrel,xbody)≈0 \beta = \arctan\left(\frac{v_{\text{rel},y}^{\text{body}}}{v_{\text{rel},x}^{\text{body}}}\right) \approx 0 β=arctan(vrel,xbodyvrel,ybody)≈0

其中 vrelbodyv_{\text{rel}}^{\text{body}}vrelbody 是 FRD（Forward-Right-Down）机体坐标系下相对于风的速度，xxx 轴指向机头，yyy 轴指向右侧，zzz 轴指向下方。侧滑角 β\betaβ 描述的是来流方向在机体 xyxyxy 平面上的投影与 xxx 轴的偏离：β=0\beta = 0β=0 表示来流正对机头；β>0\beta > 0β>0 表示来流从右侧吹入。

在分离模式下，这个约束无法使用，因为：

attitude_estimator_q 不知道风速和地速，无法计算 vrelv_{\text{rel}}vrel
LPE 虽然知道地速，但不知道准确的姿态，无法将地速转到机体坐标系

在 EKF2 的统一框架下，姿态、速度、风速都在同一个状态空间内，侧滑角约束可以直接作为观测施加，通过卡尔曼增益同时修正姿态（主要是航向）和风速。这是分离模式无论如何都无法实现的。

关于"精度更高"的客观表述 ：统一解算在理论上提供了更多的信息交叉流动通道，但"精度更高"取决于具体场景和参数调优。EKF2 的优势不在于某个单一指标，而在于系统级的信息利用效率------同样的传感器输入，统一框架能提取更多的约束关系。如果传感器质量差或参数调错，EKF2 完全可能比调好的分离模式更差。本文不比较绝对精度，只分析架构差异的机理。

3. EKF2 的核心架构

3.1 误差状态卡尔曼滤波（ESKF）

EKF2 不是标准 EKF，而是 Error-State Kalman Filter（ESKF）。两者的区别在于状态表示方式：

标准 EKF ：直接估计状态的绝对值 x^\hat{\mathbf{x}}x^，误差定义为 δx=x−x^\delta \mathbf{x} = \mathbf{x} - \hat{\mathbf{x}}δx=x−x^
ESKF ：估计标称状态（nominal state）xˉ\bar{\mathbf{x}}xˉ 和误差状态 δx\delta \mathbf{x}δx，真实状态表示为 x=xˉ⊕δx\mathbf{x} = \bar{\mathbf{x}} \oplus \delta \mathbf{x}x=xˉ⊕δx

对于四元数姿态，这种表示特别自然。EKF2 的标称状态预测采用右乘约定：

qˉk+1=qˉk⊗δqk \bar{\mathbf{q}}_{k+1} = \bar{\mathbf{q}}k \otimes \delta\mathbf{q}{k} qˉk+1=qˉk⊗δqk

其中 qˉk\bar{\mathbf{q}}_kqˉk 是当前标称四元数，δqk\delta\mathbf{q}_kδqk 是 IMU 积分得到的增量旋转。这与 PX4 的 Quaternion 类乘法定义一致：_state.quat_nominal = (_state.quat_nominal * dq).normalized()。

在误差状态的定义中，真实姿态表示为误差四元数左乘标称四元数：

qtrue=δq⊗qˉ \mathbf{q}_{\text{true}} = \delta\mathbf{q} \otimes \bar{\mathbf{q}} qtrue=δq⊗qˉ

误差旋转用小角度近似表示为 3 维矢量 δθ\delta\boldsymbol{\theta}δθ：

δq≈[1δθ/2] \delta\mathbf{q} \approx \begin{bmatrix} 1 \\ \delta\boldsymbol{\theta}/2 \end{bmatrix} δq≈[1δθ/2]

ESKF 的优势：

四元数约束自动满足：标称状态始终归一化，误差状态是小量，不需要额外约束
线性化更稳定：误差状态始终在原点附近，泰勒展开的截断误差更小
IMU 积分在标称状态上进行：高频 IMU 数据直接驱动标称状态，卡尔曼滤波只修正低频误差

3.2 代码目录结构

复制代码

src/modules/ekf2/EKF/
├── ekf.h / ekf.cpp              # 主类：状态定义、主循环
├── estimator_interface.h/.cpp   # 接口层：传感器数据输入、缓冲管理
├── control.cpp                  # 融合控制中枢：决定用哪些传感器
├── covariance.cpp               # 协方差预测与更新
├── height_control.cpp           # 高度源管理与切换
├── terrain_control.cpp          # 地形估计
├── output_predictor/            # Output Predictor
│   ├── output_predictor.h
│   └── output_predictor.cpp
├── aid_sources/                 # 各传感器融合模块
│   ├── barometer/               # 气压计
│   ├── gnss/                    # GNSS（GPS 等全球导航卫星系统）
│   ├── range_finder/            # 激光雷达/超声波
│   ├── optical_flow/            # 光流
│   ├── external_vision/         # 外部视觉（VIO、Mocap 等）
│   ├── magnetometer/            # 磁力计
│   ├── airspeed/                # 空速计
│   └── sideslip/                # 侧滑角约束
└── python/
    └── ekf_derivation/        # 符号计算与代码生成
        ├── derivation.py
        └── generated/         # 自动生成的雅可比矩阵代码

3.3 核心数据流

复制代码

IMU 数据（原始传感器 250--8000 Hz，经 vehicle_imu 整合后通常 250--1000 Hz）
    │
    ▼
[降采样至 EKF2 预测频率（约 100Hz，由 EKF2_PREDICT_US 参数控制）]
    │
    ▼
predictState() ──→ 标称状态预测（四元数积分 + 速度/位置积分）
    │
    ▼
predictCovariance() ──→ 误差协方差传播（24×24 矩阵）
    │
    ▼
controlFusionModes() ──→ 根据传感器可用性选择融合模式
    │
    ├──→ GPS fusion ──→ updateAidSourceStatus() ──→ fuse*()
    ├──→ Baro fusion ──→ ...
    ├──→ Mag fusion ──→ ...
    ├──→ Range finder fusion ──→ ...
    └──→ ...
    │
    ▼
[captureDelayedState()] ──→ 保存延迟时刻的状态
    │
    ▼
correctOutputStates() ──→ Output Predictor 对齐
    │
    ▼
输出：姿态 + 速度 + 位置（用于控制回路）

4. 时间线与延迟处理：EKF2 + Output Predictor

4.1 延迟的来源

传感器数据从产生到被 EKF2 使用，存在多层延迟：

延迟来源	典型值	说明
传感器内部处理	10~200ms	GPS 解算、视觉 SLAM、激光雷达点云处理
驱动→uORB 发布	1~5ms	内核调度、中断处理
uORB→EKF2 订阅	1~5ms	模块间通信
EKF2 内部缓冲	50~200ms	IMU 缓冲、回溯对齐

EKF2 采用延迟融合时间线 （delayed fusion time horizon）策略，核心机制是 IMU 驱动而非传感器驱动：

IMU 降采样数据触发更新 ：高频 IMU 数据（如 250Hz）经降采样后（如 100Hz）压入 _imu_buffer 环形缓冲区，同时设置 _imu_updated = true 触发 EKF 更新
取最旧 IMU 样本 ：Ekf::update() 从 _imu_buffer 中取出最旧的样本 _imu_buffer.get_oldest()，以其时间戳 _time_delayed_us 作为当前融合时间线
状态预测 ：用最旧 IMU 样本做 predictState()（四元数积分 + 速度/位置积分）和 predictCovariance()（24×24 协方差传播）
传感器对齐融合 ：controlFusionModes(imu_delayed) 用 IMU 样本的时间戳，去各传感器（GPS、磁力计、气压计等）的环形缓冲区中 pop_first_older_than(imu_delayed.time_us, ...) 查找同一时刻的数据进行融合

这种设计的本质是：IMU 按固定节奏"翻牌子"，传感器被动等待被翻。EKF2 不是等传感器到了才更新，而是每次 IMU 到了就更新，然后看有没有对应时刻的传感器数据可以融合。这保证了时间一致性，但导致 EKF2 的输出 inherently 有延迟。

4.2 EKF2 内核频率：100Hz 的取舍

EKF2 内核（predict + fuse）的运行频率是 100Hz，而非 IMU 的原始 250Hz。原因：

计算资源：24×24 协方差矩阵的预测和更新，每次涉及数千次浮点运算。在 STM32F7（216MHz）上，100Hz 已占用显著 CPU 时间。
观测频率：GPS 5~10Hz、气压计 20~50Hz、磁力计 20~100Hz。 faster than 100Hz 的融合对精度提升有限，因为观测信息没有更快到来。
数值稳定性：协方差传播的离散化精度在 10ms 步长下足够，更小步长带来的收益递减。

4.3 IMU 降采样的工程细节

EKF2 接受 250Hz IMU 数据，但在进入 EKF 内核前需要降采样至 100Hz（即每 10ms 整合一次）。整合方式值得注意：

陀螺仪降采样：

cpp 复制代码

// 在四元数空间对旋转小量累乘
_quat_nominal = _quat_nominal * delta_q;  // delta_q 是 4ms 内的微小旋转

10ms 内可能收到 2~3 个 IMU 样本，EKF2 将每个样本的 delta angle 转换为小旋转四元数，然后在四元数空间上累乘（composition）。

这不是切空间的角度累加 。切空间累加（如标准捷联惯导算法）会补偿圆锥误差（coning error），而四元数空间累乘没有这种补偿。对于消费级 MEMS 陀螺仪，10ms 内的圆锥误差通常小于 0.01°，在大多数应用场景下可忽略。但在高动态机动（如多旋翼快速翻转）时，这个近似会引入微小的姿态漂移。

加速度计降采样：

cpp 复制代码

// delta_velocity 直接累加
_delta_velocity += imu_sample.delta_vel;

加速度计的 delta velocity 在 10ms 窗口内直接累加。没有考虑旋转补偿（即没有将每个 4ms 子区间的加速度转到同一坐标系后再累加），也没有划桨误差（sculling error）补偿。

这意味着：如果飞行器在 10ms 内发生了显著姿态变化，加速度计的积分假设"坐标系不变"，会引入误差。同样，对于消费级 MEMS，这个误差通常可忽略，但高动态场景下需要注意。

关键认知 ：EKF2 的降采样不是"精度损失"，而是精度与计算成本的工程权衡。100Hz EKF 内核 + 250Hz 降采样整合，是一个经过大量飞行验证的实用组合。

4.4 Output Predictor：消除延迟的互补设计

100Hz 且带延迟的 EKF2 输出，无法满足控制器对高频姿态的实时性需求。PX4 的串级 PID 控制架构中：

位置环：50Hz
速度环：50~250Hz
角度环：250Hz

角度环需要当前时刻（或近实时）的姿态，但 EKF2 的输出是 50~200ms 之前的。

Output Predictor 的设计解决了这个问题：

复制代码

IMU (250Hz)
    │
    ├──→ EKF2 (100Hz, delayed) ──→ 高精度、有延迟的姿态估计
    │                                    │
    │                                    ▼
    └──→ Output Predictor (250Hz) ←── 对齐/修正
                │
                ▼
        实时输出（低延迟、跟踪 EKF2）

Output Predictor 是一个独立的捷联惯导（strapdown INS） ，内部有两条不同频率的回路：

回路	函数	驱动源	频率	功能
预测回路	`calculateOutputStates()`	全速率 IMU 数据	250~1000 Hz	纯 INS 积分，用当前偏置估计补偿
修正回路	`correctOutputStates()`	EKF2 延迟融合结果	~100 Hz	计算误差，修正整个历史 buffer

4.4.1 预测回路：高频纯 INS 积分

calculateOutputStates() 由 estimator_interface.cpp 在每次收到 IMU 样本时调用------不管 EKF2 是否做融合。它只做三件事：

用最新的 gyro_bias / accel_bias 补偿 delta angle / delta velocity
四元数累乘积分、速度/位置梯形积分（标准捷联惯导）
更新 _output_new（当前最新输出状态）

cpp 复制代码

// estimator_interface.cpp
_output_predictor.calculateOutputStates(
    imu_sample.time_us,
    imu_sample.delta_ang,       // 原始陀螺仪 delta angle
    imu_sample.delta_ang_dt,
    imu_sample.delta_vel,       // 原始加速度计 delta velocity
    imu_sample.delta_vel_dt);

4.4.2 修正回路：EKF2 结果驱动，不是定时驱动

correctOutputStates() 不是定时触发的，而是由 EKF2 主循环在每次完成预测和融合后显式调用：

cpp 复制代码

// ekf.cpp: 每次 EKF update() 完成时
_output_predictor.correctOutputStates(
    imu_sample_delayed.time_us,   // EKF2 融合的延迟时间戳
    _state.quat_nominal,          // EKF2 延迟姿态
    _state.vel,                   // EKF2 延迟速度
    _gpos,                        // EKF2 延迟位置
    _state.gyro_bias,
    _state.accel_bias);

修正的具体机制 （源码 output_predictor.cpp:267~356）：

Push ：把当前 _output_new 压入 _output_buffer（定长 ring buffer，长度 = _imu_buffer_length，通常 10~20 个样本，对应最大延迟时间）
取历史 ：从 buffer 取出最旧的 output_delayed（与 EKF2 融合时间戳对齐）
源码注释甚至自我质疑：
cpp 复制代码
```
// TODO: there is no guarantee that data is at delayed fusion horizon
//       Shouldnt we use pop_first_older_than?
```
当前实现用 get_oldest() 而非精确时间戳匹配，是设计上的简化。
算误差 ：计算 EKF2 状态与 output_delayed 的偏差
- 姿态误差：q_error = quat_state.inverse() * output_delayed.quat_nominal
- 速度误差：vel_err = vel_state - output_delayed.vel
- 位置误差：pos_err = pos_state - output_delayed.pos

算增益 ：增益根据实际时间延迟自适应调整

cpp 复制代码

float time_delay = time_latest_us - time_delayed_us;  // 延迟时长
float att_gain = 0.5f * _dt_update_states_avg / time_delay;
float vel_gain = _dt_correct_states_avg / vel_tau;
float pos_gain = _dt_correct_states_avg / pos_tau;

延迟越大 → 增益越小 → 修正越柔和，避免过冲。

Apply 到整个 Buffer ：把修正量加到 _output_buffer 的所有历史条目上

cpp 复制代码

// 遍历整个 buffer，每个历史状态都加上同样的 vel_correction / pos_correction
for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
    _output_buffer[index].vel += vel_correction;
    _output_buffer[index].pos += pos_correction;
}
_output_new = _output_buffer.get_newest();  // 最新状态因此也被修正

姿态修正不同 ：不是加delta，而是在 calculateOutputStates() 的下一步积分时，通过 _delta_angle_corr 间接注入。

4.4.3 关键问题的答案

Q: correctOutputStates 是 FIFO buffer 溢出驱动，还是 EKF 计算结果驱动？

A: EKF 计算结果驱动。 每次 EKF2 完成一帧预测+融合，就调用一次 correctOutputStates()。Buffer 是定长 ring buffer，满了自动覆盖最旧数据，不存在"溢出触发"的概念。

Q: 计算频率是多少？

A: 预测回路 250~1000 Hz（等于原始 IMU 频率），修正回路 ~100 Hz（等于 EKF2 融合频率）。

Q: 每来一个延迟计算结果算一次，还是 FIFO 溢出一个数据算一次？

A: 每来一个 EKF2 延迟结果就算一次。 EKF2 每帧 push 一个新状态进 buffer，然后立刻做修正。修正的粒度是"每帧 EKF 融合"，而不是"每溢出一次"。

4.4.4 为什么这样设计？

这个设计的精妙之处在于：

EKF2 负责精度：利用所有传感器做最优估计，容忍延迟
Output Predictor 负责实时性：利用高频 IMU 做纯预测，保证低延迟
两者互补：Output Predictor 的漂移被 EKF2 的定期对齐所约束，不会发散
修正历史而非外推：不是把 delta 从延迟时刻"外推"到当前时刻，而是直接把修正量加到整个历史轨迹上，这样当前时刻的输出自然也被修正，且没有时间延迟

参考文献：A. Khosravian 等人的 "Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements"（Australian National University）。

4.4.5 为什么 EKF2 不设计成 LPE 那样的延迟卡尔曼？

LPE 也做了延迟处理：_xDelay 保存历史状态，传感器到达时查历史状态 x0 算创新（r = y - C * x0），保证时间对齐。但 LPE 的修正做的是当前状态 ------它用历史状态算创新，然后用当前的协方差 P 和增益 K 去更新当前状态 _x。这是一种工程近似：它假设协方差从延迟时刻到现在没怎么变。

EKF2 为什么不这样做？因为 LPE 的近似在 EKF2 的 24 维误差状态和更大延迟下不够用了。

问题 1：协方差历史不能忽略

LPE 只有 10 维，延迟 200ms 最多跨 2~3 个历史步（LPE 跑 50~250Hz），协方差变化确实不大。EKF2 的 24×24 协方差矩阵在 100ms 内会显著演化------姿态误差会扩散到速度、位置、偏置，交叉协方差非零项大量增长。用当前 P 去修正延迟时刻的创新，增益矩阵是错的。

问题 2：多传感器延迟不一致

LPE 的传感器延迟相对单一（主要是 GPS）。EKF2 要同时处理 GPS（200ms）、光流（50ms）、气压计（几毫秒）、磁力计（几毫秒），每个传感器有自己的延迟。如果像 LPE 那样"当前 P + 历史 x"分别修正，每个传感器都引入一次近似误差，累积后协方差失去一致性。

问题 3：当前时刻没有可用输出

LPE 修正完当前状态后直接输出，控制器拿到的是当前时刻估计。EKF2 如果也这样，修正的是 200ms 前的状态（因为 GPS 延迟 200ms），当前时刻的状态仍然是纯 IMU 外推------这和 Output Predictor 做的没有任何区别。既然当前输出必然是纯 IMU 外推，不如把它显式分离成一个独立模块。

EKF2 的做法

干脆彻底一点：

所有数据进 buffer（IMU + 传感器），在延迟时间戳上做完整的 predict + correct
协方差也随历史严格传播，不近似
当前输出交给 Output Predictor，用纯 IMU 从延迟时刻外推到现在，定期用 EKF2 的最优估计修正历史轨迹

一句话总结 ：LPE 的延迟处理是"历史算创新、当前做修正"的工程近似，在 10 维小延迟场景够用；EKF2 的 24 维大延迟场景下，近似误差会累积放大，所以干脆在延迟时刻做完整融合，当前输出由 Output Predictor 负责。两套系统的区别不是"有没有延迟处理"，而是延迟处理精确到什么程度。

5. EKF2 的能力边界

在深入各子系统之前，需要建立对 EKF2 能力边界的正确认知：

5.1 EKF2 能做什么

统一估计姿态、速度、位置、偏置、磁场、风速、地形
自动选择最优传感器组合：根据可用性和质量动态切换高度源、位置源、航向源
利用传感器交叉约束：速度修正姿态、磁场修正航向、侧滑角修正风速
故障检测与降级：单个传感器失效时自动隔离，进入纯预测模式（dead reckoning）

5.2 EKF2 不能做什么

不能替代传感器校准：磁力计的硬铁/软铁校准、加速度计的六面校准必须在 EKF2 外完成
不能处理极端动态：圆锥误差和划桨误差未补偿，极限机动下姿态积分精度下降
不能实时输出无延迟高精度：延迟和精度是 trade-off，Output Predictor 的纯预测输出精度低于 EKF2
不能自动调参：24 维误差状态的 Q/R 矩阵参数众多，调参仍依赖工程师经验

5.3 与 LPE 的直接对比

维度	LPE	EKF2	差异
状态维度	10	24（误差状态）/ 25（原始 storage）	EKF2 多姿态、磁场、风、地形
姿态估计	外供（互补滤波）	内估计（ESKF）	EKF2 统一框架
磁场估计	无	地磁+机体磁	EKF2 可学习磁环境
风速估计	无	空速计+侧滑角	EKF2 支持固定翼
地形估计	tz 状态	独立 terrain	EKF2 地形与高度解耦
延迟处理	历史状态缓冲	延迟时间线+Output Predictor	EKF2 更系统
协方差更新	简化形式	约瑟夫形式	EKF2 数值更稳定
符号计算	手写	SymForce 自动生成	EKF2 可维护性更高
参数数量	~20	~80	EKF2 调参更复杂
CPU 占用	~5%	~15-25%	EKF2 计算成本更高

关于我们：

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

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