2. 导航基础
在进入 ArduPilot EKF3 的代码与参数之前，必须先建立一套统一的“导航语言”。EKF3 本质上是一个状态估计器，它把不同来源、不同频率、不同误差特性的传感器数据融合成一组连续、平滑、可用的位姿估计。如果对惯性导航、卫星导航、组合导航以及坐标系定义理解不清，后续看日志、调参数、排查漂移时就会失去判断依据。
一、三种导航方式的基本定位
惯性导航（INS）依靠 IMU 中的加速度计和陀螺仪，通过积分推算位置、速度和姿态。它的优点是输出频率高、短时平滑、不依赖外部信号；缺点是误差随时间累积，加速度计零偏会带来位置二次漂移，陀螺零偏会带来姿态缓慢发散。因此惯性导航适合“短时高动态”，不适合“长时间绝对定位”。
卫星导航（GNSS）通过接收机解算伪距、多普勒等信息，输出经纬度、高度、地速和航向。它的优点是长期绝对精度稳定、无累积误差；缺点是更新率低（常见 5～10 Hz）、易受遮挡和多路径影响、高度精度明显弱于水平精度，且信号丢失时无法独立维持导航。
组合导航则是把 INS 与 GNSS 的优势互补：用 GNSS 的长期绝对信息去约束 INS 的漂移，用 INS 的高频平滑输出填补 GNSS 的低频与丢星间隙。EKF3 正是这一思想的工程实现，它并不“切换”传感器，而是持续进行概率意义上的加权融合。
二、坐标系定义：EKF3 的“空间基准”
ArduPilot 中必须牢记三套坐标系，它们决定了数据符号与旋转方向：
- 机体坐标系（Body Frame，FRD）：X 轴指向机头前方，Y 轴指向右侧，Z 轴指向下方。IMU 原始数据默认就在此坐标系下，加速度计静止时 Z 轴读数约为 +9.8 m/s²。
- 导航坐标系（NED）：X 轴指北，Y 轴指东，Z 轴指地。EKF3 输出的位置、速度、姿态均以 NED 为参考，高度向上为正时需要取负。
- 地球坐标系（ECEF / LLH）：GNSS 原始观测量基于地心地固坐标系，最终转换为经纬高（Latitude/Longitude/Height）供任务使用。
姿态描述上，欧拉角直观但存在万向节死锁，因此 EKF3 内部使用四元数维护姿态，输出时再转换为 Roll/Pitch/Yaw。理解这一点，才能在日志中正确解读姿态跳变。
三、组合导航的融合逻辑与关键术语
EKF3 的融合过程可概括为“预测—更新”循环：IMU 到来时做状态预测并放大协方差；GNSS、气压、磁力计、光流等观测到来时做观测更新并收缩协方差。核心术语包括：
| 术语 | 含义 | 工程意义 |
|---|---|---|
| 状态向量 | 位置、速度、姿态、零偏等 | EKF3 估计的核心对象 |
| 协方差矩阵 P | 状态不确定度 | 决定融合权重与收敛速度 |
| 过程噪声 Q | 预测阶段不确定度增长 | 过大导致漂移敏感，过小导致滞后 |
| 观测噪声 R | 传感器测量可信度 | 由 EK3_*_NOISE 参数控制 |
| 新息（Innovation） | 观测值与预测值之差 | 卡方检验判据，用于剔除异常观测 |
| 零偏（Bias） | 陀螺/加计缓慢漂移量 | EKF3 在线估计并补偿 |
四、典型场景与常见误区
以多旋翼定点悬停为例：GNSS 提供水平位置约束，气压计提供高度约束，磁力计提供航向约束，IMU 提供高频姿态与速度预测。当飞机短暂穿过桥洞丢失 GNSS 时，EKF3 依靠 IMU 继续推算，位置会在数秒内缓慢漂移；一旦 GNSS 恢复，新息检验通过后重新收敛。这正是组合导航“短期靠惯性、长期靠卫星”的直观体现。
NKF1 中的 PN/PE/PD、VN/VE/VD 与 SV、SP 等字段，结合 GPS 与 IMU 消息的时间戳对齐，可快速判断是观测异常还是预测发散。
# 常用 EKF3 相关参数（示例）
EK3_ENABLE = 1 # 启用 EKF3
EK3_IMU_MASK = 1 # 使用 IMU0
EK3_GPS_TYPE = 3 # GPS 用于 3D 速度与位置
EK3_ALT_SOURCE = 0 # 高度源：气压计优先
EK3_MAG_CAL = 3 # 磁力计校准模式
掌握以上概念后，后续分析 EKF3 的预测方程、观测模型与参数整定时，就能把每一个数字对应到真实的物理量与坐标系中，而不是停留在“调参试错”的层面。