PX4 EKF2 源码解析（四）：从卡尔曼滤波到非线性状态估计
发布时间：2026/8/24 21:39:26 作者：尧图编辑部 阅读量：1,286
PX4 EKF2 源码解析四从卡尔曼滤波到非线性状态估计摘要本文建立阅读 EKF2 源码所需的最小数学框架说明状态预测、协方差预测、创新、卡尔曼增益和观测更新的关系并特别解释 PX4 创新符号与常见教材定义相反时源码中的减法为何仍与标准卡尔曼更新一致。1. 状态空间模型离散系统写成xkf(xk−1,uk)wk,zkh(xk)vk x_kf(x_{k-1},u_k)w_k, \qquad z_kh(x_k)v_kxkf(xk−1,uk)wk,zkh(xk)vk其中 (x) 是状态(u) 是 IMU 增量(z) 是观测(w\sim\mathcal N(0,Q)) 和 (v\sim\mathcal N(0,R)) 分别表示过程噪声与观测噪声。2. 线性卡尔曼滤波五个核心量阶段公式物理意义状态预测(\hat x_k^-F\hat x_{k-1})动力学外推协方差预测(P_k-FPFTQ)不确定度传播并增加过程噪声创新协方差(SHP-HTR)预测与测量差值应有的方差卡尔曼增益(KP-HTS^{-1})各状态接受观测修正的权重后验更新(\hat x\hat x^-K(z-H\hat x^-))由观测修正预测EKF 用非线性函数 (f,h) 预测均值在当前线性化点计算雅可比F∂f∂x∣x^,H∂h∂x∣x^ F\left.\frac{\partial f}{\partial x}\right|_{\hat x}, \qquad H\left.\frac{\partial h}{\partial x}\right|_{\hat x}F∂x∂fx^,H∂x∂hx^3. PX4 的创新符号教材常定义残差为 (rz-h(\hat x))PX4 多数融合代码定义innovationh(x^)−z−r \text{innovation}h(\hat x)-z-rinnovationh(x^)−z−r因此源码状态更新使用减法_state.vel-K.slice3,1(4,0)*innovation;数学上x^−K[h(x^)−z]x^K[z−h(x^)] \hat x-K[h(\hat x)-z]\hat xK[z-h(\hat x)]x^−K[h(x^)−z]x^K[z−h(x^)]二者完全等价。分析日志创新方向时必须使用 PX4 约定否则会把传感器正偏差判断成负偏差。4. 为什么经常使用标量顺序融合三维速度可一次进行矩阵更新也可依次融合 N、E、D 三个标量。PX4 大量观测使用后者观测向量 z[zN,zE,zD] ├─ 更新 N 分量 → 得到新的 x、P ├─ 更新 E 分量 → 基于更新后的 x、P └─ 更新 D 分量 → 基于更新后的 x、P当观测噪声近似不相关时顺序融合可以避免小矩阵求逆并便于对单轴观测做质量控制。若观测噪声存在显著相关性则必须先去相关或采用完整矩阵形式。5. 变量与源码对应数学量PX4 形式主要位置(x)_statecommon.h::stateSample(P)SquareMatrix24f Pekf.h(Q)IMU 噪声与状态过程噪声covariance.cpp(H)Vector24f H或生成表达式各*_fusion.cpp(R)观测方差各 aid source 更新函数(S)innovation_varianceestimator_aid_source*(K)Vector24f K融合函数6. 统计门限标量观测的归一化创新平方通常写为NISν2S \mathrm{NIS}\frac{\nu^2}{S}NISSν2若门限为 (g) 个标准差则 PX4 的检验比可理解为test ratioν2g2S \text{test ratio}\frac{\nu^2}{g^2S}test ratiog2Sν2test_ratio 1表示观测超出配置的一致性门限。它不是“传感器必然损坏”的证明还可能由状态预测错误、延迟错误或噪声参数过小引起。7. 更新数据流渲染错误:Mermaid 渲染失败: Parse error on line 2: ...测状态 x-] -- H[预测观测 h(x-)] Z[测量 z] -- -----------------------^ Expecting SQE, DOUBLECIRCLEEND, PE, -), STADIUMEND, SUBROUTINEEND, PIPE, CYLINDEREND, DIAMOND_STOP, TAGEND, TRAPEND, INVTRAPEND, UNICODE_TEXT, TEXT, TAGSTART, got PS8. EKF 线性化在导航问题中的具体含义姿态旋转是 EKF2 最典型的非线性。以磁力计预测为例z^mRbn(q)mImB \hat z_mR_{bn}(q)m_Im_Bz^mRbn(q)mImB预测观测直接使用完整四元数计算而观测雅可比描述当前四元数附近的小变化如何影响三轴磁预测。EKF 并不是把非线性模型整体替换成线性模型而是均值传播/预测观测使用完整非线性 f(x)、h(x) 协方差传播/卡尔曼增益使用当前点雅可比 F、H当初始航向相差接近 180°、位置相差很大或观测长期失锁时小误差线性化不再可靠。EKF2 的重置逻辑正是用于把状态重新带回局部线性化有效范围。9. 协方差更新形式的源码选择教科书常给出简化式P(I−KH)P− P^(I-KH)P^-P(I−KH)P−数值分析中也常使用 Joseph 形式P(I−KH)P−(I−KH)TKRKT P^(I-KH)P^-(I-KH)^TKRK^TP(I−KH)P−(I−KH)TKRKTv1.14.3measurementUpdate()使用与标量最优增益等价的PP−−KSKT P^P^--KSK^TPP−−KSKT其中 (S) 为标量创新方差。源码用KS(row) * K(col)构造外积并在更新前检查对角方差是否会为负更新后强制对称。该实现利用标量融合结构降低运算量但要求传入的 (K) 和 (S) 来自同一个观测模型。10. 观测噪声和过程噪声不能互换噪声进入方程的位置表示内容调大后的直接结果过程噪声 (Q)协方差预测模型传播未知量(P^-) 增长更快后续更信观测观测噪声 (R)创新方差传感器/观测模型误差(S) 增大当次更少信观测例如 GNSS 速度创新在机动时存在固定相位差真实原因可能是延迟参数错误。增大 (R) 会降低卡尔曼增益和检验比但相位差仍存在增大 (Q) 会让状态更容易被错误时刻的 GNSS 拉动。二者都不能修复时间模型。11. 从 aid source 反算更新过程日志给出innovation、innovation_variance、observation_variance和test_ratio后可进行以下一致性检查检查 (SR0)。若 (SR)说明日志字段、单位或实现理解有误计算归一化创新 (\nu/\sqrt S)查看其均值和标准差根据 gate 验证test_ratio是否与 (\nu2/(g2S)) 一致对连续成功融合区间统计创新均值理想状态接近零对创新做自相关检查明显长相关通常表示模型或延迟未充分描述。单次创新处于门限内只能说明该次观测未显著异常不能证明长期统计一致性。12. 可观性与卡尔曼增益的关系某状态是否出现在观测函数中只是直接可观性的第一层。以 GNSS 速度为例(H) 对速度状态是直接单位映射但姿态和偏差仍可通过 (P) 的交叉项获得增益KiPi,vPv,vR K_i\frac{P_{i,v}}{P_{v,v}R}KiPv,vRPi,v若P(i,v)接近零该观测不会明显修正第 (i) 个状态若运动模型持续建立相关性则间接修正增强。这解释了为什么相同 GNSS 数据在静止、转弯和持续加速阶段对偏差估计效果不同。13. 线性化与顺序融合次序顺序融合多维观测时第一个分量已经修改 (x) 和 (P)第二个分量原则上应在新状态附近重新计算非线性预测。不同融合文件会依据计算成本和非线性强度选择是否更新雅可比。阅读源码时需要确认三个轴是否共享一次计算的 (H)每轴融合后是否更新预测观测观测噪声是否假设轴间独立任一轴拒绝是否阻止其他轴融合fused标志表示全部轴还是至少一轴成功。这些细节决定多维 aid source 的统计解释不能只由函数名fuseVelocity()推断。14. EKF 一致性依赖的假设EKF2 的统计量可解释为概率置信度需要近似满足过程与观测噪声零均值、方差参数合理、测量与状态时间对齐、雅可比在线性化范围有效、连续观测中的相关性没有被严重重复计算。实际传感器不完全满足这些假设因此源码加入 gate、质量检查、状态抑制和 reset。尤其需要注意“同源相关观测”。VIO 的位置和速度可能来自同一个滑窗优化器GNSS 位置与速度也可能共享接收机内部状态。如果 EKF 把它们当完全独立观测连续融合后验协方差可能过小。工程上可通过更保守观测噪声、选择部分观测分量或显式建模相关性缓解。一致性评价不能只比较估计误差均方根。一个滤波器可能轨迹误差较小但方差更小统计上仍过度自信另一个轨迹略噪却能用正确方差覆盖真实误差。NIS、NEES、reset 频率和故障恢复时间应与 RMSE 一并评估。NEES 需要独立真值 (x_{true})((x-\hat x)TP{-1}(x-\hat x))。实飞通常缺少完整真值可在仿真中使用 NEES在实飞中以 NIS、多传感器交叉验证和重复航线统计作为替代证据。源码解读卡尔曼五个量在 EKF2 中的位置1. 状态均值和协方差不是同一结构文件EKF/ekf.h、EKF/common.h。stateSample _state{};SquareMatrix24f P;_state使用具名字段表达物理量P使用固定索引表达误差统计。状态预测分别落在predictCovariance(imu_sample_delayed);predictState(imu_sample_delayed);即 (P) 和 (x) 由两个函数传播。阅读时不要在predictState()中寻找F*P*Fᵀ。2. 创新在 aid source 中保存以速度/位置融合为例vel_pos_fusion.cpp的updateVelocityAidSrcStatus()负责把观测与当前状态比较并写入aid_src.observation aid_src.observation_variance aid_src.innovation aid_src.innovation_variance aid_src.test_ratio这一步对应公式中的 (z)、(R)、(\nu)、(S)。它只更新统计状态不一定修改_state。只有控制器判断门限和融合模式通过后才调用fuseVelocity()。3. 创新符号由状态减观测确认直接位置观测在源码中等价于innovation_state.pos(axis)-observation;因此状态注入使用_state.pos-K.slice3,1(7,0)*innovation;若状态预测 N 位置为 10 mGNSS 观测为 12 m则 innovation-2 m。位置对应的卡尔曼增益为正state - K*(-2)使状态向 12 m 增加与标准 KF 一致。4. 通用测量更新实现文件EKF/ekf.h::measurementUpdate()。boolmeasurementUpdate(Vector24fK,floatinnovation_variance,floatinnovation){clearInhibitedStateKalmanGains(K);constVector24f KSK*innovation_variance;SquareMatrix24f KHP;for(unsignedrow0;row_k_num_states;row){for(unsignedcol0;col_k_num_states;col){KHP(row,col)KS(row)*K(col);}}constboolis_healthycheckAndFixCovarianceUpdate(KHP);if(is_healthy){P-KHP;fixCovarianceErrors(true);fuse(K,innovation);}returnis_healthy;}逐行映射代码数学含义clearInhibited...对不可观状态人为令 (K_i0)KS K*S形成外积左向量KHP KS*Kᵀ利用标量观测恒等式得到 (KSK^T)P - KHP后验协方差更新fuse(K, innovation)(xx–K\nu)5. 门限判断不在通用内核中measurementUpdate()不接收 gate也不计算test_ratio说明创新门限属于各观测融合函数。典型调用链update aid source └─ 计算 ν、S、test_ratio │ ├─ ratio 1rejected不调用更新内核 └─ ratio ≤ 1计算K并调用 measurementUpdate这一区分对初学者很重要若观测没有修正状态应先检查控制/门限分支是否调用了内核而不是直接怀疑fuse()。6. 从日志回到公式假设日志包含innovation -2.0 innovation_variance 0.75 observation_variance 0.25 gate 5可由源码公式复算test ratio(−2)252×0.750.213 test\ ratio\frac{(-2)^2}{5^2\times0.75}0.213testratio52×0.75(−2)20.213若日志fusedtrue继续到measurementUpdate()若fusedfalse应检查数据是否新鲜、control flag、其他轴失败或协方差健康检查。15. 小结EKF2 的各种传感器融合虽然观测模型不同但最终都可归结为预测观测、计算创新及其方差、进行一致性检验、计算增益并修正状态。掌握这一公共结构后复杂源码可以拆解为“数学模型”和“工程状态机”两部分。
尧图网站开发 