PX4状态估计技术EKF2详解（四）：EKF2 Output Predictor——从延迟估计到实时输出

EKF2 Output Predictor：从延迟估计到实时输出

前一篇讲了 EKF2 如何在延迟时间戳上做融合，生成"过去"的最优估计。但飞控控制器需要的是"现在"的姿态、速度和位置。Output Predictor 就是这座桥梁------它用高频 IMU 做纯惯导积分，定期用 EKF2 的延迟结果修正历史轨迹，让控制器拿到既实时又跟踪 EKF2 精度的状态。

1. 为什么需要 Output Predictor

1.1 延迟与实时性的根本矛盾

EKF2 的设计决定了它的输出天然带有延迟：

IMU buffer 取最旧样本做预测，默认延迟约 100 ms
GPS 延迟 100~200 ms（RTK 可低至 50~100 ms），EV 延迟 30~200 ms（取决于算法和硬件）
所有融合在延迟时间戳上完成

这意味着 EKF2 的 _state 永远是 t - delay 时刻的状态。如果直接把它给控制器：

复制代码

控制器 request @ t = 500 ms
EKF2 输出 state @ t = 400 ms（延迟 100 ms）
控制器基于 100 ms 前的姿态做 PID → 相位滞后 → 震荡

PX4 串级 PID 的角度环运行在 250 Hz （4 ms 周期），要求姿态更新的延迟远小于 10 ms。EKF2 的 100 ms 延迟对此是致命的。

1.2 两条极端路径都不行

路径 A：直接用 EKF2 延迟输出

精度高（融合所有传感器）
延迟大（100+ ms）
控制器震荡，不可用

路径 B：纯 IMU 外推

零延迟（实时）
漂移快（陀螺偏置导致姿态几分钟内发散）
精度低，长期不可用

1.3 Output Predictor 的互补设计

Output Predictor 走了第三条路：

复制代码

EKF2（100Hz，延迟）──────→ 高精度、有延迟的最优估计
         │                              │
         │ 定期修正历史轨迹              │
         ▼                              ▼
Output Predictor（250Hz，实时）──→ 控制器（姿态/速度/位置环）

EKF2 负责精度：利用所有传感器在延迟时间戳上做最优估计
Output Predictor 负责实时性：利用高频 IMU 从延迟时刻外推到当前时刻
修正机制保证跟踪：EKF2 的延迟结果定期修正 Output Predictor 的历史轨迹，抑制纯 IMU 漂移

核心洞察 ：Output Predictor 不是"另一个估计器"，而是 EKF2 的输出放大器------把 EKF2 的慢速、高精度、延迟状态，转换成高频、低延迟、跟踪 EKF2 的实时状态。

2. 架构：两条频率不同的回路

Output Predictor 内部有两条独立运行的回路：

回路	函数	驱动源	频率	功能
预测回路	`calculateOutputStates()`	vehicle_imu 原始数据	250 Hz	纯 INS 积分，bias 补偿 + 残差注入
修正回路	`correctOutputStates()`	EKF2 延迟融合结果	~100 Hz	计算残差，修正整个历史 buffer

2.1 预测回路：每次 IMU 到达都运行

预测回路由 estimator_interface.cpp 在每次收到 IMU 数据时调用 ------无论 EKF2 是否做融合。输入为 vehicle_imu 输出的 delta angle Δ θ k \Delta\boldsymbol{\theta}_k Δθk 和 delta velocity Δ v k \Delta\mathbf{v}_k Δvk（已在传感器驱动层整合，通常 250 Hz）。这是 Output Predictor 的"心跳"，250 Hz 不间断。

2.2 修正回路：EKF2 融合完成才触发

修正回路在 EKF2 完成每帧 predict + fuse 后被调用，输入为 EKF2 在延迟时间戳 t delay t_{\text{delay}} tdelay 上的最优估计 ( q ˉ , v ˉ , p ˉ ) (\bar{\mathbf{q}}, \bar{\mathbf{v}}, \bar{\mathbf{p}}) (qˉ,vˉ,pˉ) 以及对应的偏置估计 ( b ^ g , b ^ a ) (\hat{\mathbf{b}}_g, \hat{\mathbf{b}}_a) (b^g,b^a)。频率 ≈ EKF2 内核频率（~100 Hz），但不是定时触发------仅在融合完成后调用。

3. 预测回路：高频纯捷联惯导积分

3.1 输入与补偿

预测回路在积分前对 IMU 测量做两步补偿。记陀螺仪 delta angle 为 Δ θ k \Delta\boldsymbol{\theta}_k Δθk，加速度计 delta velocity 为 Δ v k \Delta\mathbf{v}k Δvk，对应的积分时间为 Δ t ω \Delta t\omega Δtω 和 Δ t a \Delta t_a Δta。

Step 1：陀螺仪偏置补偿

陀螺仪偏置 b ^ g \hat{\mathbf{b}}_g b^g 和加速度计偏置 b ^ a \hat{\mathbf{b}}_a b^a 均来自 EKF2 在延迟时刻的估计，由修正回路每帧更新。基于"短时间内偏置变化极小"的假设（~100 ms），直接用延迟估计近似当前偏置：

Δ θ k corr = Δ θ k − b ^ g Δ t ω + δ θ corr \Delta\boldsymbol{\theta}_k^{\text{corr}} = \Delta\boldsymbol{\theta}k - \hat{\mathbf{b}}g \Delta t\omega + \boldsymbol{\delta}\theta^{\text{corr}} Δθkcorr=Δθk−b^gΔtω+δθcorr

其中 δ θ corr \boldsymbol{\delta}_\theta^{\text{corr}} δθcorr 来自修正回路（见 4.2 节），是姿态跟踪 EKF2 的角增量修正项。

Step 2：加速度计偏置补偿

Δ v k corr = Δ v k − b ^ a Δ t a \Delta\mathbf{v}_k^{\text{corr}} = \Delta\mathbf{v}_k - \hat{\mathbf{b}}_a \Delta t_a Δvkcorr=Δvk−b^aΔta

3.2 姿态更新：四元数累乘

将补偿后的角增量转为 delta quaternion：

δ q k = [ cos ⁡ ∥ Δ θ k corr ∥ 2 Δ θ k corr ∥ Δ θ k corr ∥ sin ⁡ ∥ Δ θ k corr ∥ 2 ] \delta\mathbf{q}_k = \begin{bmatrix} \cos\frac{\|\Delta\boldsymbol{\theta}_k^{\text{corr}}\|}{2} \\ \frac{\Delta\boldsymbol{\theta}_k^{\text{corr}}}{\|\Delta\boldsymbol{\theta}_k^{\text{corr}}\|} \sin\frac{\|\Delta\boldsymbol{\theta}_k^{\text{corr}}\|}{2} \end{bmatrix} δqk=[cos2∥Δθkcorr∥∥Δθkcorr∥Δθkcorrsin2∥Δθkcorr∥]

姿态更新采用标准捷联惯导的右乘约定：

q ˉ k + 1 = q ˉ k ⊗ δ q k , q ˉ k + 1 ← q ˉ k + 1 ∥ q ˉ k + 1 ∥ \bar{\mathbf{q}}{k+1} = \bar{\mathbf{q}}k \otimes \delta\mathbf{q}k, \quad \bar{\mathbf{q}}{k+1} \leftarrow \frac{\bar{\mathbf{q}}{k+1}}{\|\bar{\mathbf{q}}{k+1}\|} qˉk+1=qˉk⊗δqk,qˉk+1←∥qˉk+1∥qˉk+1

注意 δ θ corr \boldsymbol{\delta}_\theta^{\text{corr}} δθcorr 已被包含在 Δ θ k corr \Delta\boldsymbol{\theta}_k^{\text{corr}} Δθkcorr 中------修正回路的姿态修正不是"立即旋转存量四元数"，而是"注入到下一步积分的角增量中"，使修正与 IMU 数据自然融合。

3.3 速度更新：旋转 + 重力补偿

用最新四元数构造旋转矩阵 R k + 1 = R ( q ˉ k + 1 ) \mathbf{R}{k+1} = \mathbf{R}(\bar{\mathbf{q}}{k+1}) Rk+1=R(qˉk+1)，将补偿后的 delta velocity 从机体坐标系（body）转到 NED 坐标系，并补偿重力加速度（ g g g 沿 NED 的 z z z 轴正方向）：

Δ v k NED = R k + 1 Δ v k corr + g Δ t a , g = [ 0 0 g ] \Delta\mathbf{v}k^{\text{NED}} = \mathbf{R}{k+1} \Delta\mathbf{v}_k^{\text{corr}} + \mathbf{g} \Delta t_a, \quad \mathbf{g} = \begin{bmatrix} 0 \\ 0 \\ g \end{bmatrix} ΔvkNED=Rk+1Δvkcorr+gΔta,g= 00g

速度更新：

v ˉ k + 1 = v ˉ k + Δ v k NED \bar{\mathbf{v}}_{k+1} = \bar{\mathbf{v}}_k + \Delta\mathbf{v}_k^{\text{NED}} vˉk+1=vˉk+ΔvkNED

3.4 位置更新：梯形积分

位置更新采用梯形积分，比欧拉积分更精确：

p ˉ k + 1 = p ˉ k + v ˉ k + 1 + v ˉ k 2 Δ t a \bar{\mathbf{p}}_{k+1} = \bar{\mathbf{p}}k + \frac{\bar{\mathbf{v}}{k+1} + \bar{\mathbf{v}}_k}{2} \Delta t_a pˉk+1=pˉk+2vˉk+1+vˉkΔta

在 250 Hz 的高频下，梯形积分的截断误差 O ( Δ t 3 ) O(\Delta t^3) O(Δt3) 已足够小。

3.5 IMU 安装偏移补偿

若 IMU 安装位置偏离机体原点 r IMU body \mathbf{r}{\text{IMU}}^{\text{body}} rIMUbody，角速度 ω \boldsymbol{\omega} ω 会产生切向速度 （tangential velocity） ω × r IMU body \boldsymbol{\omega} \times \mathbf{r}{\text{IMU}}^{\text{body}} ω×rIMUbody。该速度定义在机体坐标系（body），需经旋转矩阵 R k + 1 \mathbf{R}_{k+1} Rk+1（body→NED）转到导航坐标系：

v IMU NED = R k + 1 ( ω × r IMU body ) \mathbf{v}{\text{IMU}}^{\text{NED}} = \mathbf{R}{k+1} \left( \boldsymbol{\omega} \times \mathbf{r}_{\text{IMU}}^{\text{body}} \right) vIMUNED=Rk+1(ω×rIMUbody)

Output Predictor 在输出速度和位置时减去该偏移，保证控制器拿到的是机体原点处的状态。

向心加速度呢？

旋转刚体上任意点的完整加速度包含三项：
a IMU = a body + α × r ⏟ 切向 + ω × ( ω × r ) ⏟ 向心 \mathbf{a}{\text{IMU}} = \mathbf{a}{\text{body}} + \underbrace{\boldsymbol{\alpha} \times \mathbf{r}}{\text{切向}} + \underbrace{\boldsymbol{\omega} \times (\boldsymbol{\omega} \times \mathbf{r})}{\text{向心}} aIMU=abody+切向 α×r+向心 ω×(ω×r)

加速度计测量的是 IMU 所在位置的比力，因此向心加速度和切向加速度已经包含在 delta velocity 中 。但 Output Predictor 的积分过程没有在加速度层面显式减去向心项------原因有三：

量级通常很小 ：多旋翼偏航角速度 ω ≈ 1 \omega \approx 1 ω≈1 rad/s，偏移 r ≈ 0.05 r \approx 0.05 r≈0.05 m，向心加速度 a = ω 2 r ≈ 0.05 a = \omega^2 r \approx 0.05 a=ω2r≈0.05 m/s²，仅为重力的 0.5%

被偏置状态吸收 ：这类低频、与运动状态耦合的误差，被 EKF2 的加速度计偏置 b a \mathbf{b}_a ba 部分吸收

工程权衡 ：显式补偿需要角加速度 α \boldsymbol{\alpha} α（角速度微分），噪声大，补偿收益低

例外：固定翼大机动、高转速旋翼机、或 IMU 装在翼尖等场景，向心加速度可达重力的 5~10%，此时仅靠偏置吸收不够，需要更精细的杆臂效应补偿（lever-arm compensation）。

4. 修正回路：互补滤波修正历史轨迹

修正回路是 Output Predictor 的灵魂。它不直接融合观测，而是把 EKF2 的延迟最优估计当作"真值"，计算与自身历史预测的残差，然后用互补滤波把修正量 apply 到整个历史 buffer。

4.1 整体流程

修正回路的五个步骤：

保存 EKF2 偏置估计 ( b ^ g , b ^ a ) (\hat{\mathbf{b}}_g, \hat{\mathbf{b}}_a) (b^g,b^a) 供预测回路使用
将当前预测状态 ( q ˉ , v ˉ , p ˉ ) t now (\bar{\mathbf{q}}, \bar{\mathbf{v}}, \bar{\mathbf{p}}){t{\text{now}}} (qˉ,vˉ,pˉ)tnow 压入 ring buffer
取 buffer 中最旧状态 ( q ˉ , v ˉ , p ˉ ) t delay (\bar{\mathbf{q}}, \bar{\mathbf{v}}, \bar{\mathbf{p}}){t{\text{delay}}} (qˉ,vˉ,pˉ)tdelay，与 EKF2 融合时间戳对齐
分别计算姿态、速度、位置的残差与修正量
将修正量 apply 到整个历史 buffer

4.2 姿态修正：注入角增量的互补滤波

姿态修正不是直接旋转当前四元数 ，而是计算一个角增量修正量 δ θ corr \boldsymbol{\delta}_\theta^{\text{corr}} δθcorr，供下一次预测回路积分时注入。

Step 1：计算四元数残差

记 EKF2 延迟时刻的最优姿态为 q ˉ EKF \bar{\mathbf{q}}{\text{EKF}} qˉEKF，Output Predictor 在同一延迟时刻的历史预测姿态为 q ˉ OP \bar{\mathbf{q}}{\text{OP}} qˉOP。四元数残差为：

q err = q ˉ EKF − 1 ⊗ q ˉ OP \mathbf{q}{\text{err}} = \bar{\mathbf{q}}{\text{EKF}}^{-1} \otimes \bar{\mathbf{q}}_{\text{OP}} qerr=qˉEKF−1⊗qˉOP

q err \mathbf{q}{\text{err}} qerr 表示从 EKF2 姿态到 Output Predictor 姿态所需的相对旋转。若两者完全对齐， q err = [ 1 , 0 , 0 , 0 ] T \mathbf{q}{\text{err}} = [1, 0, 0, 0]^T qerr=[1,0,0,0]T。

Step 2：小角度近似转换为角增量误差

设 q err = [ w , x , y , z ] T \mathbf{q}_{\text{err}} = [w, x, y, z]^T qerr=[w,x,y,z]T，在小角度假设下：

δ θ err = 2 sign ( w ) [ x y z ] \boldsymbol{\delta}_\theta^{\text{err}} = 2 \, \text{sign}(w) \begin{bmatrix} x \\ y \\ z \end{bmatrix} δθerr=2sign(w) xyz

这是四元数小角度误差的标准转换：旋转向量 ≈ 2 × \approx 2 \times ≈2× 虚部。

Step 3：计算姿态修正增益

Δ t delay = t now − t delay , k att = 1 2 Δ t IMU Δ t delay \Delta t_{\text{delay}} = t_{\text{now}} - t_{\text{delay}}, \quad k_{\text{att}} = \frac{1}{2} \frac{\Delta t_{\text{IMU}}}{\Delta t_{\text{delay}}} Δtdelay=tnow−tdelay,katt=21ΔtdelayΔtIMU

Δ t delay \Delta t_{\text{delay}} Δtdelay：当前时刻与 EKF2 延迟时刻的时间差（~100 ms）
Δ t IMU \Delta t_{\text{IMU}} ΔtIMU：IMU 平均更新周期（~4 ms @ 250 Hz）
典型值： k att ≈ 0.5 × 0.004 / 0.1 = 0.02 k_{\text{att}} \approx 0.5 \times 0.004 / 0.1 = 0.02 katt≈0.5×0.004/0.1=0.02

增益的物理意义：延迟越大 → 增益越小 → 修正越柔和，避免过冲。 k att ∝ 1 / Δ t delay k_{\text{att}} \propto 1/\Delta t_{\text{delay}} katt∝1/Δtdelay 的设计保证了约 0.7 的阻尼比，与延迟配置无关。

Step 4：生成角增量修正量

δ θ corr = k att ⋅ δ θ err \boldsymbol{\delta}\theta^{\text{corr}} = k{\text{att}} \cdot \boldsymbol{\delta}_\theta^{\text{err}} δθcorr=katt⋅δθerr

δ θ corr \boldsymbol{\delta}_\theta^{\text{corr}} δθcorr 被保存于 Output Predictor 内部，下一次预测回路做四元数积分时注入（见 3.1 节）。

关键设计：姿态修正不是"立即修改存量四元数"，而是"修改下一次积分的增量"。这样做的好处是：

避免四元数直接叠加导致的数值问题

修正量与 IMU 数据自然融合，输出更平滑

互补滤波的行为更直观------相当于给陀螺仪附加了一个"虚拟校正角速度"

4.3 水平速度/位置修正：PI 控制器

速度和位置的修正采用互补滤波思想，数学上实现为 PI 控制器。

Step 1：计算跟踪误差

记 EKF2 延迟时刻的最优速度和位置为 v ˉ EKF \bar{\mathbf{v}}{\text{EKF}} vˉEKF、 p ˉ EKF \bar{\mathbf{p}}{\text{EKF}} pˉEKF，Output Predictor 在同一延迟时刻的历史预测为 v ˉ OP \bar{\mathbf{v}}{\text{OP}} vˉOP、 p ˉ OP \bar{\mathbf{p}}{\text{OP}} pˉOP：

e v = v ˉ EKF − v ˉ OP , e p = p ˉ EKF − p ˉ OP \mathbf{e}v = \bar{\mathbf{v}}{\text{EKF}} - \bar{\mathbf{v}}{\text{OP}}, \quad \mathbf{e}p = \bar{\mathbf{p}}{\text{EKF}} - \bar{\mathbf{p}}{\text{OP}} ev=vˉEKF−vˉOP,ep=pˉEKF−pˉOP

Step 2：计算增益

k v = Δ t EKF τ v , k p = Δ t EKF τ p k_v = \frac{\Delta t_{\text{EKF}}}{\tau_v}, \quad k_p = \frac{\Delta t_{\text{EKF}}}{\tau_p} kv=τvΔtEKF,kp=τpΔtEKF

Δ t EKF \Delta t_{\text{EKF}} ΔtEKF：EKF2 修正的平均周期（~10 ms）
τ v = τ p = 0.25 \tau_v = \tau_p = 0.25 τv=τp=0.25 s（默认时间常数）
典型值： k v ≈ k p ≈ 0.01 / 0.25 = 0.04 k_v \approx k_p \approx 0.01 / 0.25 = 0.04 kv≈kp≈0.01/0.25=0.04

Step 3：积分项累加

s v ← s v + e v , s p ← s p + e p \mathbf{s}_v \leftarrow \mathbf{s}_v + \mathbf{e}_v, \quad \mathbf{s}_p \leftarrow \mathbf{s}_p + \mathbf{e}_p sv←sv+ev,sp←sp+ep

积分状态 s v \mathbf{s}_v sv、 s p \mathbf{s}_p sp 用于消除稳态误差（如 GPS 长期漂移）。

Step 4：计算 PI 修正量

Δ v corr = k v e v + 0.1 k v 2 s v \Delta\mathbf{v}^{\text{corr}} = k_v \mathbf{e}_v + 0.1 k_v^2 \mathbf{s}_v Δvcorr=kvev+0.1kv2sv

Δ p corr = k p e p + 0.1 k p 2 s p \Delta\mathbf{p}^{\text{corr}} = k_p \mathbf{e}_p + 0.1 k_p^2 \mathbf{s}_p Δpcorr=kpep+0.1kp2sp

P 项 k v e v k_v \mathbf{e}_v kvev：比例响应，快速跟踪 EKF2
I 项 0.1 k v 2 s v 0.1 k_v^2 \mathbf{s}_v 0.1kv2sv：积分响应，消除稳态误差

积分增益取 0.1 k v 2 0.1 k_v^2 0.1kv2 的经验系数，保证 I 项增长速度远慢于 P 项，避免积分饱和。

Step 5：Apply 到整个 Buffer

修正量被加到 ring buffer 的所有历史条目上：

v ˉ i ← v ˉ i + Δ v corr , p ˉ i ← p ˉ i + Δ p corr , ∀ i ∈ buffer \bar{\mathbf{v}}_i \leftarrow \bar{\mathbf{v}}_i + \Delta\mathbf{v}^{\text{corr}}, \quad \bar{\mathbf{p}}_i \leftarrow \bar{\mathbf{p}}_i + \Delta\mathbf{p}^{\text{corr}}, \quad \forall i \in \text{buffer} vˉi←vˉi+Δvcorr,pˉi←pˉi+Δpcorr,∀i∈buffer

为什么修正整个 buffer，而不是只修正当前？

历史一致性 ：buffer 中保存了从 t_delay 到 t_now 的完整轨迹。如果只修正 _output_new（当前），那么 buffer 中旧的状态仍然偏离 EKF2，下一次 get_oldest() 取出的历史状态就会与 EKF2 对不上，残差计算基础被破坏。
当前输出自然正确 ：_output_new = _output_buffer.get_newest()，修正整个 buffer 后，最新状态自动也被修正。
无时间延迟：不是"从延迟时刻外推到当前"，而是"直接把修正量加到整个历史轨迹上"，当前时刻的输出自然正确。

4.4 垂直方向修正：PD 控制器

垂直方向（高度）采用与水平方向不同的控制结构------PD 控制器而非 PI。

记 EKF2 延迟时刻的高度为 h ˉ EKF \bar{h}{\text{EKF}} hˉEKF，垂直速度为 v ˉ z , EKF \bar{v}{z,\text{EKF}} vˉz,EKF；Output Predictor 的历史预测高度为 h ˉ OP \bar{h}{\text{OP}} hˉOP（由 vert_vel_integ 维护），垂直速度为 v ˉ z , OP \bar{v}{z,\text{OP}} vˉz,OP。

Step 1：计算高度和速度残差

e h = h ˉ EKF − h ˉ OP , e v z = v ˉ z , EKF − v ˉ z , OP e_h = \bar{h}{\text{EKF}} - \bar{h}{\text{OP}}, \quad e_{v_z} = \bar{v}{z,\text{EKF}} - \bar{v}{z,\text{OP}} eh=hˉEKF−hˉOP,evz=vˉz,EKF−vˉz,OP

Step 2：PD 修正量

Δ v z corr = k p e h + 1.1 k v e v z \Delta v_z^{\text{corr}} = k_p e_h + 1.1 k_v e_{v_z} Δvzcorr=kpeh+1.1kvevz

e h e_h eh 是位置误差（P 项）
e v z e_{v_z} evz 是速度误差（D 项）
没有积分项------PX4 的高度传感器（气压计、距离计、激光雷达）更新快、延迟低、可观测性强，PD 已能快速稳定跟踪

Step 3：梯形积分传播修正

垂直方向维护两套状态： v z v_z vz（垂直速度）和 h h h（高度积分）。修正时不仅修改速度，还要用梯形积分重新传播高度：

v z , i + 1 ← v z , i + 1 + Δ v z corr v_{z,i+1} \leftarrow v_{z,i+1} + \Delta v_z^{\text{corr}} vz,i+1←vz,i+1+Δvzcorr

h i + 1 = h i + v z , i + v z , i + 1 2 Δ t i − Δ h ref h_{i+1} = h_i + \frac{v_{z,i} + v_{z,i+1}}{2} \Delta t_i - \Delta h_{\text{ref}} hi+1=hi+2vz,i+vz,i+1Δti−Δhref

其中 Δ h ref \Delta h_{\text{ref}} Δhref 是全局参考点变化（如 GPS 原点跳变时的补偿）。遍历整个 buffer 后，速度和高度始终保持一致。

5. Buffer 体系与对齐精度

5.1 Ring Buffer 结构

Output Predictor 有两个 ring buffer：

长度 = _imu_buffer_length（默认 11，与 IMU buffer 相同）
保存从 t delay t_{\text{delay}} tdelay（约 100 ms 前）到 t now t_{\text{now}} tnow（当前）的预测状态轨迹
每次修正回路运行时 push 当前状态，满时自动覆盖最旧数据

5.2 `get_oldest()` 的近似性

Output Predictor 用 get_oldest() 取最旧状态，但不保证这个时间戳恰好等于 EKF2 融合的时间戳 t delay t_{\text{delay}} tdelay。理想情况下应该用 pop_first_older_than(t_{\text{delay}}) 做精确匹配。

为什么没改？因为：

Output Predictor buffer 和 IMU buffer 长度相同、push 频率相同（每次 EKF2 update() 同时 push）
在大多数时刻，get_oldest() 确实是精确对齐的
只有当调度抖动导致 push 不同步时，才会有微小偏差（通常 < 10 ms）

5.3 `_output_new` 的特殊角色

在两次修正回路触发之间（约 10 ms），预测回路每收到一帧 IMU 就更新当前状态 q ˉ t now \bar{\mathbf{q}}{t{\text{now}}} qˉtnow，但不写入 ring buffer。这意味着：

当前状态是实时的最新预测，直接供给控制器
Ring buffer 只保存过去的历史轨迹（用于与 EKF2 对齐）
控制器通过 getQuaternion()、getVelocity()、getLatLonAlt() 等接口读取当前状态

6. 参数与调参

Output Predictor 的可调参数很少，核心是两个时间常数：

速度修正时间常数： τ v = 0.25 \tau_v = 0.25 τv=0.25 s
位置修正时间常数： τ p = 0.25 \tau_p = 0.25 τp=0.25 s

6.1 增益与时间常数的关系

k v = Δ t EKF τ v , k p = Δ t EKF τ p k_v = \frac{\Delta t_{\text{EKF}}}{\tau_v}, \quad k_p = \frac{\Delta t_{\text{EKF}}}{\tau_p} kv=τvΔtEKF,kp=τpΔtEKF

Δ t EKF ≈ 10 \Delta t_{\text{EKF}} \approx 10 ΔtEKF≈10 ms（EKF2 修正周期）
默认 τ v = τ p = 0.25 \tau_v = \tau_p = 0.25 τv=τp=0.25 s
默认增益 k v ≈ k p ≈ 0.04 k_v \approx k_p \approx 0.04 kv≈kp≈0.04

时间常数的物理意义：Output Predictor 跟踪 EKF2 的响应速度 。tau 越小 → 增益越大 → 跟踪越快，但可能对 EKF2 的噪声更敏感；tau 越大 → 跟踪越慢，但更平滑。

6.2 姿态增益的不可调性

姿态增益 att_gain 是自动计算的，不可手动设置：

k att = 1 2 Δ t IMU Δ t delay k_{\text{att}} = \frac{1}{2} \frac{\Delta t_{\text{IMU}}}{\Delta t_{\text{delay}}} katt=21ΔtdelayΔtIMU

这保证了无论用户配置多大的 ekf2_delay_max，姿态修正的阻尼比始终约为 0.7------既快速跟踪又不过冲。

6.3 调参建议

绝大多数场景不需要调 Output Predictor 的参数。只有在以下特殊情况下可能需要调整：

tau 太小（< 0.1 s）：Output Predictor 过度跟踪 EKF2 的噪声，控制器感受到高频抖动
tau 太大（> 1 s）：Output Predictor 跟踪 EKF2 太慢，长时间漂移后修正幅度大，控制器感受到低频拉扯
默认 0.25 s 是经过大量飞行验证的平衡点

7. 小结

Output Predictor 的本质：EKF2 的实时输出放大器。

维度	EKF2	Output Predictor
频率	~100 Hz	250 Hz（等于 vehicle_imu）
延迟	有（~100 ms）	无（实时）
精度来源	多传感器融合	EKF2 定期修正 + 纯 IMU 积分
核心算法	延迟卡尔曼滤波	捷联惯导 + 互补滤波
输出对象	Output Predictor	控制器

三条关键机制：

预测回路 ：每次 IMU 到达就做纯 INS 积分（bias 补偿 + _delta_angle_corr 注入），保证 250 Hz 实时输出
修正回路：EKF2 每帧融合完成后，计算延迟时刻的残差，用 PI/PD 互补滤波修正整个历史 buffer
姿态修正的特殊性：不直接旋转四元数，而是把修正量注入下一次积分的角增量------平滑、无突变、数值稳定

与参考论文的联系 ：Output Predictor 的互补滤波思想来自 A. Khosravian 等人的 "Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements"（Australian National University）。核心洞见是：当估计器有延迟时，用高频预测 + 低频修正的互补结构，可以同时满足精度和实时性。

关于我们：

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

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