4. 扩展卡尔曼滤波原理
在 ArduPilot 的 EKF3 中，状态量包含姿态四元数、速度、位置、陀螺零偏、加速度计零偏、地磁偏置、风场等，而 IMU 的比力观测、磁力计观测、GPS 位置速度观测与状态之间往往不是线性关系。标准卡尔曼滤波只能处理线性高斯系统，因此 EKF3 采用扩展卡尔曼滤波（Extended Kalman Filter, EKF），其核心思想是：在当前状态估计点附近对非线性模型做一阶泰勒展开，用雅可比矩阵把非线性问题局部线性化，再套用卡尔曼滤波的递推框架。
一、非线性系统线性化：从真实模型到局部线性模型
设系统状态为 x，连续时间非线性过程模型与观测模型为：
x_k = f(x_{k-1}, u_k) + w_k // 过程模型，w_k ~ N(0, Q)
z_k = h(x_k) + v_k // 观测模型，v_k ~ N(0, R)其中 f 描述姿态积分、速度位置递推等非线性动力学，h 描述传感器如何由状态生成观测。EKF 并不改变 f 和 h 本身，而是在上一时刻后验估计 x̂_{k-1} 处做一阶展开：
f(x) ≈ f(x̂) + F·(x - x̂)
h(x) ≈ h(x̂) + H·(x - x̂)其中 F = ∂f/∂x、H = ∂h/∂x 就是雅可比矩阵。线性化后，误差传播近似为线性高斯过程，卡尔曼滤波的均值与协方差递推公式即可直接使用。需要强调的是，EKF 的“最优”只是局部近似意义下的最优，状态离展开点越远，一阶近似误差越大。
二、雅可比矩阵：EKF 的“线性化引擎”
雅可比矩阵是 EKF 区别于 KF 的关键。对 n 维状态、m 维观测，F 是 n×n 矩阵，H 是 m×n 矩阵，其元素为各状态分量的一阶偏导。在 ArduPilot EKF3 中，雅可比通常不解析求导，而是通过状态转移的代数结构直接构造，或对四元数、旋转矩阵等做小角度近似。
以姿态四元数为例，姿态误差常用旋转向量 δθ 表示，四元数更新可近似为：
q_new ≈ q ⊗ [1, 0.5·δθ]^T
δθ ≈ 2 · vec(q_err)这种“误差状态”参数化让姿态雅可比保持近似线性，同时避免四元数归一化约束带来的奇异。EKF3 还使用协方差矩阵的平方根形式（如 Cholesky 或 UD 分解）来更新，以提升数值稳定性。
三、EKF 递推流程：预测与更新交替
EKF 每个滤波周期分为预测（时间更新）和更新（观测更新）两步。预测利用 IMU 等高频数据推进状态和协方差；更新利用 GPS、磁力计、气压计等低频观测修正状态。其标准递推如下：
- 状态预测：
x̂_k⁻ = f(x̂_{k-1}, u_k) - 协方差预测：
P_k⁻ = F·P_{k-1}·Fᵀ + Q - 计算卡尔曼增益：
K = P_k⁻·Hᵀ·(H·P_k⁻·Hᵀ + R)⁻¹ - 状态更新：
x̂_k = x̂_k⁻ + K·(z_k - h(x̂_k⁻)) - 协方差更新：
P_k = (I - K·H)·P_k⁻
其中 z_k - h(x̂_k⁻) 称为新息（innovation），它反映观测与预测的偏差。EKF3 会对新息做卡方检验，若新息过大则判定观测异常并拒绝该观测，这就是常见的“创新门限”保护。
四、数值稳定性：EKF 工程落地的生命线
EKF 的公式看似简单，但在嵌入式飞控上长期运行，数值问题会直接导致姿态发散或位置跳变。ArduPilot EKF3 在实现中重点处理以下几类稳定性问题：
- 协方差对称性：理论上
P应始终对称正定，但浮点舍入会破坏对称性。EKF3 在每次更新后强制对称化，或使用平方根滤波保持正定。 - 四元数归一化：姿态四元数在积分后会偏离单位长度，必须周期归一化，否则旋转矩阵会出现尺度误差。
- 雅可比近似误差：大角度机动时一阶近似失效，EKF3 通过限制单步旋转角度、提高 IMU 更新频率来减小线性化误差。
- 观测异常与门限：GPS 多路径、磁干扰会造成新息突变，卡方检验和 innovation gate 可避免污染状态。
- 数值尺度差异：位置（米）、速度（米/秒）、零偏（弧度/秒）量级差异大，需合理设置
Q、R和初始P，避免矩阵病态。
五、参数与调试要点
| 参数/机制 | 作用 | 典型调试方向 |
|---|---|---|
EK3_GBIAS_PNOISE |
陀螺零偏过程噪声 | 过大导致姿态漂移，过小导致收敛慢 |
EK3_ABIAS_PNOISE |
加速度计零偏过程噪声 | 影响速度与高度估计的跟随性 |
EK3_MAG_PNOISE |
磁力计观测噪声 | 磁干扰环境下适当增大 |
EK3_GLITCH_RAD |
新息门限半径 | 过小拒绝正常观测，过大放过异常 |
EK3_POS_I_GATE |
位置观测创新门限 | GPS 跳变时保护位置状态 |
六、案例复盘：GPS 跳变下的 EKF3 行为
某多旋翼在城区飞行时，GPS 水平位置突然跳变 15 米。EKF3 的预测状态仍由 IMU 推进，位置新息 y 瞬间增大，卡方检验超过 EK3_POS_I_GATE 设定门限，该帧 GPS 位置观测被拒绝，状态未被污染。随后 GPS 恢复正常，新息回到门限内，滤波器重新融合观测。若门限设置过大，跳变会被直接吸收，导致位置估计被拉偏，进而影响高度和速度控制。
XKF1、XKF2、XKF3、XKF4 日志中的新息、协方差和状态量，结合 NKF1 的 PI、PN 等字段判断是预测发散还是观测异常。不要只凭飞行手感调参。
总结来看，EKF3 的可靠性来自三件事：用雅可比矩阵把非线性模型局部线性化，用预测-更新递推持续融合多源观测，用数值稳定性和门限机制守住工程底线。理解这三者的关系，才能在 ArduPilot 中正确配置和排查组合导航问题。