本文是《PX4 位置速度估计技术详解》系列的第四篇。建议先阅读第三篇：传感器融合与测量更新，了解标准卡尔曼更新的完整流程。
第三篇讨论了 GPS 测量更新的标准卡尔曼校正方程：
r = y − C x ^ , K = P C T S − 1 , x ^ ← x ^ + K r \mathbf{r} = \mathbf{y} - \mathbf{C}\hat{\mathbf{x}}, \quad \mathbf{K} = \mathbf{P}\mathbf{C}^T\mathbf{S}^{-1}, \quad \hat{\mathbf{x}} \leftarrow \hat{\mathbf{x}} + \mathbf{K}\mathbf{r} r=y−Cx^,K=PCTS−1,x^←x^+Kr
但这个方程隐含了一个关键假设：测量值 y \mathbf{y} y 和状态估计 x ^ \hat{\mathbf{x}} x^ 是同一时刻的。对于 GPS、气压计等延迟可忽略的传感器，这个假设成立。但对于视觉 SLAM、激光雷达点云等传感器，数据从采集到传输到飞控可能需要数百毫秒。当测量到达时，状态已经向前推进了数十步------此时直接套用标准卡尔曼更新，会引入系统性偏差。
本篇从**延迟卡尔曼滤波（Delayed Kalman Filter, DKF）**的理论出发，推导严格最优解，分析 LPE 采用的工程近似，并逐行对照源码，理解延迟补偿在嵌入式系统中的实现细节。
1. 问题建模：延迟测量是什么？
1.1 标准卡尔曼更新的时间对齐假设
标准离散时间卡尔曼滤波假设测量与状态在时间上严格对齐：
y k = C k x k + ν k \mathbf{y}_k = \mathbf{C}_k \mathbf{x}_k + \nu_k yk=Ckxk+νk
其中 y k \mathbf{y}_k yk 是 t k t_k tk 时刻的测量， x k \mathbf{x}k xk 是 t k t_k tk 时刻的状态。滤波器在第 k k k 步的预测状态 x ^ k ∣ k − 1 \hat{\mathbf{x}}{k|k-1} x^k∣k−1 与测量 y k \mathbf{y}_k yk 是"同时"的，新息：
r k = y k − C k x ^ k ∣ k − 1 \mathbf{r}_k = \mathbf{y}_k - \mathbf{C}k \hat{\mathbf{x}}{k|k-1} rk=yk−Ckx^k∣k−1
具有明确的统计意义------它衡量的是"同一时刻下，测量与预测的偏差"。
1.2 延迟测量的数学描述
设传感器在时刻 t d t_d td（delayed）采集数据，但数据到达飞控的时刻为 t k t_k tk（current），且 t d < t k t_d < t_k td<tk。定义延迟时间：
τ = t k − t d > 0 \tau = t_k - t_d > 0 τ=tk−td>0
延迟测量的数学形式为：
y d = C d x d + ν d \mathbf{y}_d = \mathbf{C}_d \mathbf{x}_d + \nu_d yd=Cdxd+νd
其中下标 d d d 表示"delayed"，即测量对应的是过去时刻 t d t_d td 的状态 x d = x ( t d ) \mathbf{x}_d = \mathbf{x}(t_d) xd=x(td)，而非当前时刻 t k t_k tk 的状态 x k = x ( t k ) \mathbf{x}_k = \mathbf{x}(t_k) xk=x(tk)。
1.3 延迟破坏了哪个公式：新息协方差还是增益？
如果忽略延迟，直接用当前状态计算新息：
r = y d − C d x ^ k \mathbf{r} = \mathbf{y}_d - \mathbf{C}_d \hat{\mathbf{x}}_k r=yd−Cdx^k
这个式子 comparing apples to oranges------左边是 t d t_d td 时刻的测量，右边是 t k t_k tk 时刻的状态预测。两者之间隔着 τ \tau τ 秒的系统动力学演化，新息中混入了"状态自然变化"的系统性分量，不再纯粹反映测量误差。
更深层的问题在于标准卡尔曼增益公式不能直接套用。让我们从推导开始，看延迟破坏了什么。
标准卡尔曼的新息：
r k = y k − C k x ^ k ∣ k − 1 = C k x ~ k ∣ k − 1 + ν k \mathbf{r}_k = \mathbf{y}_k - \mathbf{C}k \hat{\mathbf{x}}{k|k-1} = \mathbf{C}k \tilde{\mathbf{x}}{k|k-1} + \boldsymbol{\nu}_k rk=yk−Ckx^k∣k−1=Ckx~k∣k−1+νk
新息协方差的展开式：
S k = C k E [ x ~ k ∣ k − 1 x ~ k ∣ k − 1 T ] ⏟ P k ∣ k − 1 C k T + E [ ν k ν k T ] ⏟ R k + C k E [ x ~ k ∣ k − 1 ν k T ] ⏟ = 0 + E [ ν k x ~ k ∣ k − 1 T ] ⏟ = 0 C k T \mathbf{S}k = \mathbf{C}k \underbrace{E[\tilde{\mathbf{x}}{k|k-1}\tilde{\mathbf{x}}{k|k-1}^T]}{\mathbf{P}{k|k-1}} \mathbf{C}_k^T + \underbrace{E[\boldsymbol{\nu}_k \boldsymbol{\nu}k^T]}{\mathbf{R}k} + \mathbf{C}k \underbrace{E[\tilde{\mathbf{x}}{k|k-1} \boldsymbol{\nu}k^T]}{=\,0} + \underbrace{E[\boldsymbol{\nu}k \tilde{\mathbf{x}}{k|k-1}^T]}{=\,0} \mathbf{C}_k^T Sk=CkPk∣k−1 E[x~k∣k−1x~k∣k−1T]CkT+Rk E[νkνkT]+Ck=0 E[x~k∣k−1νkT]+=0 E[νkx~k∣k−1T]CkT
交叉项为零的关键前提：状态预测误差 x ~ k ∣ k − 1 \tilde{\mathbf{x}}_{k|k-1} x~k∣k−1 与当前测量噪声 ν k \boldsymbol{\nu}_k νk 独立 。这是因为 x ~ k ∣ k − 1 \tilde{\mathbf{x}}{k|k-1} x~k∣k−1 只包含 t k − 1 t{k-1} tk−1 时刻之前的信息，而 ν k \boldsymbol{\nu}_k νk 是 t k t_k tk 时刻的噪声，两者没有重叠。
由此得到简洁公式：
S k = C k P k ∣ k − 1 C k T + R k \mathbf{S}_k = \mathbf{C}k \mathbf{P}{k|k-1} \mathbf{C}_k^T + \mathbf{R}_k Sk=CkPk∣k−1CkT+Rk
但在延迟情况下，事情变了 。LPE 用历史状态 x ^ d \hat{\mathbf{x}}_d x^d 计算新息（这是正确的）：
r d = y d − C d x ^ d = C d x ~ d + ν d \mathbf{r}_d = \mathbf{y}_d - \mathbf{C}_d \hat{\mathbf{x}}_d = \mathbf{C}_d \tilde{\mathbf{x}}_d + \boldsymbol{\nu}_d rd=yd−Cdx^d=Cdx~d+νd
这个新息的方差 本身没有问题： S d = C d P d ∣ d − 1 C d T + R d \mathbf{S}_d = \mathbf{C}d \mathbf{P}{d|d-1} \mathbf{C}_d^T + \mathbf{R}_d Sd=CdPd∣d−1CdT+Rd。
问题出在增益公式。标准卡尔曼增益：
K = P s t a t e C T S − 1 \mathbf{K} = \mathbf{P}_{state} \mathbf{C}^T \mathbf{S}^{-1} K=PstateCTS−1
这里的 P s t a t e \mathbf{P}{state} Pstate 是"被更新状态"的协方差。标准情况下，被更新的是 x ^ k ∣ k − 1 \hat{\mathbf{x}}{k|k-1} x^k∣k−1，所以 P s t a t e = P k ∣ k − 1 \mathbf{P}{state} = \mathbf{P}{k|k-1} Pstate=Pk∣k−1，与 S \mathbf{S} S 中的 P \mathbf{P} P 是同一个矩阵。
但在延迟情况下，我们是用历史新息 r d \mathbf{r}_d rd 去更新当前状态 x ^ k \hat{\mathbf{x}}_k x^k。严格最优的增益应该是：
K o p t = cov [ x ~ k , x ~ d ] ⏟ P k , d C d T ( C d P d ∣ d − 1 C d T + R d ) − 1 ⏟ S d − 1 \mathbf{K}_{opt} = \underbrace{\text{cov}[\tilde{\mathbf{x}}k, \tilde{\mathbf{x}}d]}{\mathbf{P}{k,d}} \mathbf{C}_d^T \underbrace{(\mathbf{C}d \mathbf{P}{d|d-1} \mathbf{C}_d^T + \mathbf{R}d)^{-1}}{\mathbf{S}_d^{-1}} Kopt=Pk,d cov[x~k,x~d]CdTSd−1 (CdPd∣d−1CdT+Rd)−1
分子是互协方差 P k , d \mathbf{P}_{k,d} Pk,d------衡量"当前状态误差"与"历史状态误差"的相关程度。分母是历史新息协方差 S d \mathbf{S}_d Sd。
LPE 的简化是 ：用当前协方差 P k \mathbf{P}k Pk 同时替代了分子中的互协方差 P k , d \mathbf{P}{k,d} Pk,d 和分母中的历史协方差 P d ∣ d − 1 \mathbf{P}_{d|d-1} Pd∣d−1：
K L P E = P k C d T ( C d P k C d T + R d ) − 1 \mathbf{K}_{LPE} = \mathbf{P}_k \mathbf{C}_d^T (\mathbf{C}_d \mathbf{P}_k \mathbf{C}_d^T + \mathbf{R}_d)^{-1} KLPE=PkCdT(CdPkCdT+Rd)−1
这在数学上不是等价的 。只有当 P k ≈ P k , d ≈ F k , d P d ∣ d − 1 \mathbf{P}k \approx \mathbf{P}{k,d} \approx \mathbf{F}{k,d} \mathbf{P}{d|d-1} Pk≈Pk,d≈Fk,dPd∣d−1 时，近似才成立------即延迟期间过程噪声的累积不显著改变协方差结构。这正是 LPE 简化的隐含前提。
1.4 各传感器的典型延迟
| 传感器 | 典型延迟 | 延迟来源 |
|---|---|---|
| GPS | 100~200 ms | 信号解算、数传链路 |
| 视觉/VIO | 50~500 ms | 图像处理、特征匹配、位姿解算 |
| 激光雷达 | 50~100 ms | 点云累积、扫描周期 |
| 光流 | < 20 ms | 积分时间极短 |
| 气压计 | < 10 ms | 模拟传感器，几乎无延迟 |
对于 GPS 的 100~200ms 延迟，多旋翼在悬停时位置变化约 0~2cm，速度变化约 0~0.2m/s，延迟误差尚可接受。但对于视觉 SLAM 的 300~500ms 延迟，同一飞行器可能已移动数米，延迟误差不可忽略。
2. 三层解法：从严格最优到工程近似
延迟卡尔曼滤波的研究形成了三个层次的方法，从严格最优到工程可行逐步简化。理解这三层的区别，是评估 LPE 设计选择的基础。
2.1 第一层：严格最优解（全局重处理 + Algorithm A1）
全局重处理 的思想实验：假设存储了从 t d t_d td 到 t k t_k tk 的所有测量和状态估计。当延迟测量 y d \mathbf{y}_d yd 在 t k t_k tk 时刻到达时：
- 回退到 t d t_d td 时刻的状态估计 x ^ d ∣ d − 1 \hat{\mathbf{x}}_{d|d-1} x^d∣d−1
- 用 y d \mathbf{y}d yd 执行卡尔曼更新，得到修正后的 x ^ d ∣ d \hat{\mathbf{x}}{d|d} x^d∣d
- 从 t d t_d td 到 t k t_k tk 重新传播状态和协方差
- 用重新传播的结果替换当前状态
这在数学上等价于"如果时间倒流，在 t d t_d td 时刻就收到了这个测量"。它是全局最优的，因为所有测量都按时间顺序处理，没有任何信息被丢弃或近似。
Bar-Shalom 的 Algorithm A1 （2002）在不重处理的前提下达到了相同的全局最优性。核心思想是显式补偿当前状态误差与延迟测量之间的相关性。精确解需要计算互协方差：
P k , d = cov [ x ~ k , x ~ d ] \mathbf{P}_{k,d} = \text{cov}[\tilde{\mathbf{x}}_k, \tilde{\mathbf{x}}_d] Pk,d=cov[x~k,x~d]
以及过程噪声的条件均值。最终增益形式为：
W k = P k , d C d T ( C d P d ∣ d − 1 C d T + R d ) − 1 \mathbf{W}k = \mathbf{P}{k,d} \mathbf{C}_d^T \left( \mathbf{C}d \mathbf{P}{d|d-1} \mathbf{C}_d^T + \mathbf{R}_d \right)^{-1} Wk=Pk,dCdT(CdPd∣d−1CdT+Rd)−1
为什么嵌入式系统做不到？
- 存储需求 ：互协方差 P k , d \mathbf{P}_{k,d} Pk,d 是 n x × n x n_x \times n_x nx×nx 矩阵，且需要为延迟窗口内的每个历史时刻各存一个。若最大延迟为 N N N 步，需存储 N N N 个这样的矩阵。对于 10 维状态和 50 步延迟，即 10 × 10 × 50 = 5000 10 \times 10 \times 50 = 5000 10×10×50=5000 个浮点数（约 20 KB）。
- 计算需求 ：每次收到延迟测量需更新互协方差矩阵，计算复杂度 O ( n x 3 ) O(n_x^3) O(nx3)。
- 全局重处理更不可行：需保存 50 步完整历史，每次重新执行 50 步预测+更新。
在 STM32F4/F7 的 1~2ms 计算预算内，第一层方案不可行。
2.2 第二层：文献近似解（Bar-Shalom Algorithm B1）
Bar-Shalom 在 A1 基础上提出了 Algorithm B1，一种单步近似的实用方案：
- 用历史状态计算新息： r d = y d − C d x ^ d \mathbf{r}_d = \mathbf{y}_d - \mathbf{C}_d \hat{\mathbf{x}}_d rd=yd−Cdx^d
- 计算回溯增益（retrodiction gain）：用当前协方差与历史状态的互协方差计算增益
- 将校正施加到当前状态
B1 的增益严格来说应使用历史协方差 P d ∣ d − 1 \mathbf{P}{d|d-1} Pd∣d−1 和互协方差 P k , d \mathbf{P}{k,d} Pk,d，但在单步延迟（single-step lag）的仿真中，B1 的性能通常接近 A1 。其代价是：需要存储历史协方差（ n x × n x n_x \times n_x nx×nx）和计算回溯增益的额外开销。
2.3 第三层：LPE 工程实现（简化 B）
LPE 在 B1 的基础上做了进一步工程简化：
| 简化项 | B1（文献） | LPE（工程） |
|---|---|---|
| 存储内容 | 历史状态 + 历史协方差 + 互协方差 | 仅历史状态（10 个 10 维向量） |
| 协方差采样 | 每步保存 | 50ms 间隔采样（20 Hz） |
| 增益计算 | 使用历史协方差或互协方差 | 使用当前协方差 P k \mathbf{P}_k Pk |
| 量化精度 | 精确到步长 | 50ms 量化间隔 |
这些简化将存储从 O ( n x 2 ) O(n_x^2) O(nx2) 降到 O ( n x ⋅ H I S T _ L E N ) O(n_x \cdot HIST\_LEN) O(nx⋅HIST_LEN)，在 10 维状态下仅需约 400 字节。计算上保持标准卡尔曼更新的复杂度 O ( n x 2 ⋅ n y ) O(n_x^2 \cdot n_y) O(nx2⋅ny)，额外开销仅为一次历史状态查找。
LPE 的三步流程：
- 用历史状态计算新息 （与 B1 相同）： r d = y d − C d x ^ d \mathbf{r}_d = \mathbf{y}_d - \mathbf{C}_d \hat{\mathbf{x}}_d rd=yd−Cdx^d
- 用当前协方差计算增益 （LPE 简化）： K k = P k C d T ( C d P k C d T + R d ) − 1 \mathbf{K}_k = \mathbf{P}_k \mathbf{C}_d^T (\mathbf{C}_d \mathbf{P}_k \mathbf{C}_d^T + \mathbf{R}_d)^{-1} Kk=PkCdT(CdPkCdT+Rd)−1
- 直接修正当前状态 （与 B1 相同）： x ^ k ← x ^ k + K k r d \hat{\mathbf{x}}_k \leftarrow \hat{\mathbf{x}}_k + \mathbf{K}_k \mathbf{r}_d x^k←x^k+Kkrd
3. LPE 简化的数学依据与适用边界
3.1 为什么历史新息对当前状态仍有修正价值？
延迟期间的状态演化：
x k = F k , d x d + ∑ i = d k − 1 F k , i + 1 w i \mathbf{x}k = \mathbf{F}{k,d} \mathbf{x}d + \sum{i=d}^{k-1} \mathbf{F}_{k,i+1} \mathbf{w}_i xk=Fk,dxd+i=d∑k−1Fk,i+1wi
状态估计误差：
x ~ k = F k , d x ~ d + ∑ i = d k − 1 F k , i + 1 w i \tilde{\mathbf{x}}k = \mathbf{F}{k,d} \tilde{\mathbf{x}}d + \sum{i=d}^{k-1} \mathbf{F}_{k,i+1} \mathbf{w}_i x~k=Fk,dx~d+i=d∑k−1Fk,i+1wi
历史新息 r d \mathbf{r}_d rd 包含了 x ~ d \tilde{\mathbf{x}}_d x~d 的信息。由于 x ~ k \tilde{\mathbf{x}}_k x~k 与 x ~ d \tilde{\mathbf{x}}d x~d 通过状态转移矩阵 F k , d \mathbf{F}{k,d} Fk,d 线性相关，修正 x ~ d \tilde{\mathbf{x}}_d x~d 的误差对减小 x ~ k \tilde{\mathbf{x}}_k x~k 也有帮助。
关键前提 ：延迟期间的过程噪声累积不能过大。过程噪声使 x ~ k \tilde{\mathbf{x}}_k x~k 与 x ~ d \tilde{\mathbf{x}}d x~d 的关系从确定性的 F k , d x ~ d \mathbf{F}{k,d}\tilde{\mathbf{x}}d Fk,dx~d 变成随机的 F k , d x ~ d + ∑ F k , i + 1 w i \mathbf{F}{k,d}\tilde{\mathbf{x}}d + \sum \mathbf{F}{k,i+1}\mathbf{w}_i Fk,dx~d+∑Fk,i+1wi。如果过程噪声项的方差远大于状态转移项的方差，则历史误差信息被"淹没"，修正价值急剧下降。
3.2 为什么当前协方差可以替代历史协方差？
LPE 使用 P k \mathbf{P}k Pk 而非 P d ∣ d − 1 \mathbf{P}{d|d-1} Pd∣d−1 计算增益。严格来说，增益应基于历史协方差：
K d = P d ∣ d − 1 C d T ( C d P d ∣ d − 1 C d T + R d ) − 1 \mathbf{K}d = \mathbf{P}{d|d-1} \mathbf{C}_d^T (\mathbf{C}d \mathbf{P}{d|d-1} \mathbf{C}_d^T + \mathbf{R}_d)^{-1} Kd=Pd∣d−1CdT(CdPd∣d−1CdT+Rd)−1
LPE 的简化相当于假设：
P k ≈ F k , d P d ∣ d − 1 F k , d T + Q k , d \mathbf{P}k \approx \mathbf{F}{k,d} \mathbf{P}{d|d-1} \mathbf{F}{k,d}^T + \mathbf{Q}_{k,d} Pk≈Fk,dPd∣d−1Fk,dT+Qk,d
即当前协方差已经"吸收"了从 t d t_d td 到 t k t_k tk 的过程噪声增长。由于 P k \mathbf{P}k Pk 通常大于 P d ∣ d − 1 \mathbf{P}{d|d-1} Pd∣d−1（过程噪声使协方差增长），用 P k \mathbf{P}_k Pk 计算增益是一种保守近似------增益偏小，校正量偏保守。
3.3 适用边界：什么时候有效？什么时候失效？
通常有效的场景（LPE 的设计目标）：
- 短延迟 （ τ < 500 \tau < 500 τ<500 ms）：过程噪声在短时间内的累积有限，历史误差与当前误差的相关性通常较强
- 低动态：飞行器加速度小，状态在延迟期间的变化主要由模型预测捕获
- 过程噪声不显著 ： Q \mathbf{Q} Q 的累积在 τ \tau τ 时间内不严重改变协方差结构
在单步延迟、低过程噪声的仿真条件下，Bar-Shalom 的 Algorithm B1 通常表现出与 A1 接近的性能。但需要注意，这一结论来自特定仿真条件，不能直接推广到所有场景。
可能失效的场景：
- 长延迟 + 高动态 ：飞行器在 τ \tau τ 秒内机动剧烈，历史状态与当前状态的关联性被过程噪声严重削弱
- 大过程噪声：风扰、振动等外部扰动导致状态在短时期内不可预测地变化
- 延迟超过最大补偿窗口 ：LPE 的
DELAY_MAX = 0.5s，超过此延迟的数据被丢弃，因为此时近似误差通常已不可接受
4. LPE 中的实现细节
4.1 环形缓冲区 _xDelay
LPE 使用 BlockDelay 模板类实现历史状态的环形存储：
cpp
BlockDelay<float, n_x, 1, HIST_LEN> _xDelay; // 状态历史
BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay; // 时间戳历史关键参数（BlockLocalPositionEstimator.hpp）：
cpp
static const float DELAY_MAX = 0.5f; // 最大补偿延迟：500 ms
static const float HIST_STEP = 0.05f; // 历史采样周期：50 ms (20 Hz)
static const size_t HIST_LEN = 10; // 缓冲区长度：10 = DELAY_MAX / HIST_STEPBlockDelay 的内部实现是一个固定长度的环形数组 _h[LEN]，配合写入指针 _index。update(x) 存储当前值，get(delay) 按偏移量取出历史值：
cpp
matrix::Matrix<Type, M, N> get(size_t delay)
{
int j = _index - delay;
if (j < 0) { j += LEN; }
return _h[j];
}4.2 历史状态的采样策略
历史状态不是每步预测都保存的（那样 HIST_LEN 需要很大）。LPE 以 20 Hz（50ms 间隔）采样保存：
cpp
float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);
if (_time_last_hist == 0 || (dt_hist > HIST_STEP)) {
_tDelay.update(Scalar<uint64_t>(_timeStamp));
_xDelay.update(_x);
_time_last_hist = _timeStamp;
}这意味着：
- 缓冲区保存了最近 10 个历史状态，时间跨度 0~500ms
- 相邻历史状态的时间间隔约为 50ms
- 对于 200ms 的 GPS 延迟，取出的历史状态是 200ms ± 25ms 时刻的估计（最近的采样点）
量化误差：由于历史状态是 50ms 间隔采样的，实际取出的历史状态与理论延迟时间之间存在最多 25ms 的量化误差。对于 GPS 的 200ms 延迟，相对误差约 12.5%。
4.3 延迟索引的计算：getDelayPeriods()
cpp
int BlockLocalPositionEstimator::getDelayPeriods(float delay, uint8_t *periods)
{
float t_delay = 0;
uint8_t i_hist = 0;
for (i_hist = 1; i_hist < HIST_LEN; i_hist++) {
t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i_hist)(0, 0));
if (t_delay > delay) {
break;
}
}
*periods = i_hist;
if (t_delay > DELAY_MAX) {
return -1; // 延迟超过最大补偿窗口，丢弃
}
return OK;
}算法逻辑：
- 从最近的 history（index=1）开始，逐个检查时间戳
- 找到第一个时间戳满足
t_delay > delay的历史状态 - 返回该历史状态的索引
- 如果所有历史状态的时间戳都小于
delay（即延迟超过 500ms），返回 -1
注意：getDelayPeriods 找的是满足 t_delay >= delay 的最小索引。由于历史状态是 50ms 间隔，实际取出的历史状态时间可能比目标延迟大最多 50ms。
4.4 GPS 延迟补偿的完整流程
cpp
// 1. 计算延迟索引
uint8_t i_hist = 0;
if (getDelayPeriods(_param_lpe_gps_delay.get(), &i_hist) < 0) { return; }
// 2. 取历史状态
Vector<float, n_x> x0 = _xDelay.get(i_hist);
// 3. 用历史状态计算新息
Vector<float, n_y_gps> r = y - C * x0;
// 4. 用当前协方差计算新息协方差
Matrix<float, n_y_gps, n_y_gps> S = C * m_P * C.transpose() + R;
// 5. 计算增益（使用当前协方差 m_P）
Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, n_y_gps>(S);
Matrix<float, n_x, n_y_gps> K = m_P * C.transpose() * S_I;
// 6. 更新当前状态
Vector<float, n_x> dx = K * r;
_x += dx;
m_P -= K * C * m_P;4.5 视觉传感器的特殊处理
视觉 SLAM 的延迟比 GPS 更大且不稳定，LPE 提供了两种延迟计算方式：
cpp
// 方式 1：参数设定固定延迟
float vision_delay = _param_lpe_vis_delay.get();
// 方式 2：自动计算延迟（从时间戳差）
if (vision_delay <= 0.0f) {
vision_delay = (_timeStamp - _sub_visual_odom.get().timestamp_sample) * 1e-6f;
}
// 取两者中适用的
if (getDelayPeriods(vision_delay, &i_hist) < 0) { return; }参数设定 （LPE_VIS_DELAY）：开发者手动设定视觉传感器的典型延迟。
自动计算 ：利用 uORB 消息中的 timestamp_sample（数据采集时刻）与当前时刻 _timeStamp 的差值，动态计算实际延迟。这种方式要求视觉模块在 uORB 消息中提供准确的采集时间戳。
5. 延迟参数的设置原则
5.1 GPS 延迟（LPE_GPS_DELAY）
默认值：0.2 s（200 ms）
设置方法：
- 查看飞行日志中
estimator_innovations.gps_hpos新息序列 - 如果飞行器直线飞行时新息呈现系统性滞后（先正后负或先负后正的振荡模式），说明延迟补偿不足，增大
LPE_GPS_DELAY - 如果新息无系统性滞后但偶尔跳变，默认值已足够
典型值范围：
- 数传链路好（USB/串口直连）：0.1~0.15 s
- 标准 UART 数传：0.2 s
- 无线数传（WiFi/915MHz）：0.3~0.5 s
5.2 视觉延迟（LPE_VIS_DELAY）
默认值：0.0 s（使用自动计算）
设置方法：
- 如果视觉模块提供准确的
timestamp_sample，设为 0，让 LPE 自动计算 - 如果时间戳不准确，手动设为典型值：
- 轻量级 VIO（如 MAVSDK 视觉）：0.05~0.1 s
- 中等复杂度 SLAM：0.1~0.3 s
- 重型 SLAM（如激光雷达 + 视觉融合）：0.3~0.5 s
5.3 为什么 DELAY_MAX = 0.5 s？
这是 LPE 硬编码的最大补偿窗口。设计考量：
- 上界：超过 500ms 的延迟，Algorithm B 的近似误差已显著增大。此时历史状态与当前状态的关联性被过程噪声严重削弱，用历史新息修正当前状态的收益有限。
- 下界与存储 ：
HIST_STEP = 50ms和HIST_LEN = 10的组合，使得缓冲区恰好覆盖 500ms。更小的步长需要更大的缓冲区，增加存储开销。 - 工程折中：500ms 覆盖了绝大多数传感器的延迟范围（GPS 200ms、视觉 300ms、激光雷达 100ms）。对于延迟超过 500ms 的极端场景，LPE 选择丢弃测量而非引入大误差。
6. 严格最优解的代价：为什么 LPE 不采用？
| 方案 | 存储需求 | 计算需求 | 最优性 | LPE 采用？ |
|---|---|---|---|---|
| 全局重处理 | O ( n x 2 ⋅ N d e l a y ) O(n_x^2 \cdot N_{delay}) O(nx2⋅Ndelay) | O ( N d e l a y ) O(N_{delay}) O(Ndelay) 重运行 | 全局最优 | 否 |
| Bar-Shalom A1 | O ( n x 2 ⋅ N d e l a y ) O(n_x^2 \cdot N_{delay}) O(nx2⋅Ndelay) | O ( n x 3 ) O(n_x^3) O(nx3) 互协方差更新 | 精确最优 | 否 |
| Bar-Shalom B1 | O ( n x 2 ) O(n_x^2) O(nx2) | O ( n x 2 ⋅ n y ) O(n_x^2 \cdot n_y) O(nx2⋅ny) | 近似，接近 A1（特定条件下） | 否 |
| LPE 简化 B | O ( n x ⋅ H I S T L E N ) O(n_x \cdot HIST_LEN) O(nx⋅HISTLEN) | O ( n x 2 ⋅ n y ) O(n_x^2 \cdot n_y) O(nx2⋅ny) | 工程近似 | 是 |
LPE 在 Algorithm B 的基础上进一步简化：
- 不存储互协方差，只存储历史状态
- 历史状态以 50ms 为步长采样（而非每步保存）
- 增益用当前协方差计算（而非历史协方差或插值）
这些简化将存储需求从 O ( n x 2 ) O(n_x^2) O(nx2) 降到 O ( n x ⋅ H I S T L E N ) O(n_x \cdot HIST_LEN) O(nx⋅HISTLEN)，计算需求保持 O ( n x 2 ⋅ n y ) O(n_x^2 \cdot n_y) O(nx2⋅ny)。对于 10 维状态和 6 维 GPS 测量，每次 GPS 更新的额外开销仅为一次 10 维状态查找和一次标准卡尔曼更新------在 STM32F4 上约占用 0.1~0.2ms，完全在计算预算内。
7. 小结
延迟卡尔曼滤波的核心问题是：当测量到达时，状态已经不再是测量对应的那个状态了。
LPE 的工程解决方案（Algorithm B 的简化实现）：
- 用历史状态算新息 ：从
_xDelay环形缓冲区取出 t d t_d td 时刻的状态估计，确保新息反映"同一时刻"的测量-预测偏差 - 用当前协方差算增益 ：使用 P k \mathbf{P}_k Pk 计算卡尔曼增益，是一种保守近似
- 直接修正当前状态：假设历史误差与当前误差相关，将历史新息的修正信息传递到当前状态
关键参数：
HIST_STEP = 50ms：历史状态采样间隔HIST_LEN = 10：最大存储 10 个历史状态DELAY_MAX = 500ms：超过此延迟的数据被丢弃LPE_GPS_DELAY：GPS 手动延迟补偿（默认 200ms）LPE_VIS_DELAY：视觉手动延迟补偿（默认 0，自动计算）
适用边界：短延迟（< 500ms）、低动态、过程噪声不显著的场景。对于长延迟或高动态场景，LPE 的简化近似会引入可感知的误差，此时应考虑更严格的补偿方案或降低对延迟传感器的依赖。
关于我们：
灵智实验室（LingzhiLab）成立于2020年，核心团队源自西北工业大学，由一群深耕无人系统、自动控制与机器人技术的青年工程师与科研人员组成。我们始终秉持"开放、协同、智能、可靠"的理念，致力于推动无人智能体在复杂环境下的自主感知、决策与控制能力。
实验室聚焦于基于开源飞控（如PX4）与ROS 2的深度融合，构建高可靠、模块化、可扩展的无人系统软件架构。依托扎实的工程实践与学术背景，灵智实验室积极参与开源社区建设，助力科研教育与产业落地