INAV GPS Fix Estimation 指南：固定翼航位推算与无 GPS 定点的 RTH 实现

• 无人机
• 嵌入式
• 智能硬件

本文围绕 INAV 的 GPS Fix Estimation（GPS 定位估算/航位推算）功能展开，介绍其工作原理、CLI 配置方法（inav_allow_gps_fix_estimation、fw_reference_airspeed、failsafe_gps_fix_estimation_delay）以及从遥控器禁用 GPS Fix 的测试手段，并结合 src/main/io/gps.c 等源码解析估算算法的实现细节，帮助固定翼用户理解并安全启用"丢失 GPS 后仍能 RTH"的能力。

一、功能定位：为什么需要 GPS Fix Estimation

GPS 是 INAV 中可信度最高的传感器，其输出不做验证。默认情况下，一旦丢失 GPS fix，所有导航模式都会失效；如果此时 RC 信号也丢失，飞机无法启动 RTH，只会切换为 LANDING 模式。如果飞机正在湖泊等无法落地的空域上方飞行，结果就是丢机。

GPS Fix Estimation 允许飞机在丢失 GPS fix 时，仅依靠磁力计（航向）和气压计（高度）估算自身位置，核心目标是无 GPS 定点时完成 RTH（Return To Home）。该功能仅对固定翼（fixed wing）有效，因为固定翼可以做出可靠的速度假设，而多旋翼不能。

传感器要求（飞机至少应具备）：

• 加速度计、陀螺仪（IMU）
• 气压计（barometer）
• GPS
• 磁力计（可选，但强烈建议安装）
• 皮托空速管（pitot，可选，可显著提高估算精度）

需要明确该功能的边界：

• 它不是无 GPS 导航的完整解决方案——没有 GPS 定点时位置误差会快速累积，但累积到 RTH 可用程度是可以接受的；
• 它不是应对 GPS 欺骗（spoofing）的解决方案，因为 INAV 不校验 GPS 输出；
• 飞机必须先获得 GPS 定点并保存 home 位置再起飞，全程无 GPS 的情况下估算无法工作。

此外，该功能不仅处理"丢失 fix"，还覆盖 GPS 传感器超时（硬件故障、串口断连等）的场景——从源码看，传感器超时会调用 gpsTryEstimateOnTimeout()，若满足估算条件则直接生成估算解。

二、工作原理：两个核心假设

正常情况下，飞机从 GPS 传感器获取位置，从而保持航向、执行 RTH 或按航点导航。丢失 GPS fix 后，飞机手里只剩下磁力计提供的机头航向和气压计提供的海拔高度。

要在无 GPS 状态下继续估算位置，INAV 做出两个假设：

1. 飞机正朝机头所指的方向飞行；
2. 若未安装皮托空速管，飞机以设定值（cruise airspeed）指定的恒定空速飞行。

基于这两个假设，位置可以被粗略估算。为提高精度，INAV 还会利用GPS 丢失前估算出的风速和风向来修正地速；如果安装了皮托，则直接使用皮托实测空速替代常值。

由估算的航向和速度，飞机可以粗略推算自身位置，并假定飞机将沿估算方向朝 home 点飞行，直到 GPS fix 或 RC 信号恢复。

源码中的估算算法

估算核心在 src/main/io/gps.c 的updateEstimatedGPSFix()中（约 L285-L359），其计算流程为：

1. 取速度：默认取pidProfile()->fixedWingReferenceAirspeed（即fw_reference_airspeed）；若USE_PITOT编译开启且皮托传感器健康，则用getAirspeedEstimate()实测空速；
2. 姿态转速度矢量：用姿态旋转矩阵rMat把机头方向分解为 NEU 坐标系下的水平空速(velX, velY)；
3. 叠加风修正：若isEstimatedWindSpeedValid()为真，则加上getEstimatedWindSpeed(X/Y)，由空速变为地速——这就是"利用 GPS 丢失前估算的风速"的实现；
4. 着陆特殊处理：若检测到STATE(LANDING_DETECTED)，或处于NAV_STATE_RTH_LANDING且油门为 0，则速度置零（估算位置不再移动）；
5. 积分更新经纬度：以DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR为尺度，按时间步长dt累加 lat/lon；海拔则用posControl.gpsOrigin.alt + baro.BaroAlt直接由气压计合成，因此气压计是估算的硬性依赖；
6. 伪造 GPS 解：把gpsSol.fixType置为GPS_FIX_3D，numSat/hdop置为特征值99，eph/epv置为100，随后gpsProcessNewSolutionData()检测numSat == 99便设置全局状态STATE(GPS_ESTIMATED_FIX)并清除STATE(GPS_FIX)——整个导航/定位子系统看到的就是一个"有效的 GPS 解"。

估算启动的前置条件在canEstimateGPSFix()中（src/main/io/gps.c）：

return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && sensors(SENSOR_BARO) && baroIsHealthy() && ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME);

即：设置项开启、固定翼平台、气压计可用且健康、曾锁定过（即 home 已存）、且曾经获得过 GPS 定点（GPS_FIX_HOME）。注释中还特意说明：不检查sensors(SENSOR_GPS)，因为 GPS 超时时该标志为假，而功能同样需要覆盖超时场景。

下游各模块对该状态的消费可以印证这一设计：

• 位置估计器 src/main/navigation/navigation_pos_estimator.c 的onNewGPSData()、navIsHeadingUsable()、calculateCurrentValidityFlags()中均出现STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)的判断，即估算解会像真实 GPS 数据一样参与 EKF 预测/校正流程；
• RTH 安全校验器validateRTHSanityChecker()（src/main/navigation/navigation.c）在STATE(GPS_ESTIMATED_FIX)期间主动禁用"离 home 越来越远则中止 RTH"的检查，并在真实 fix 恢复后延迟 5 秒再恢复检查——因为估算坐标跳回真实坐标时位置会突变，避免误触发 RTH 中止。

三、配置步骤（CLI）

1. 启用 GPS Fix 估算

set inav_allow_gps_fix_estimation=ON

在 src/main/fc/settings.yaml 中，该参数默认值为OFF，仅当目标以USE_GPS_FIX_ESTIMATION宏编译时才存在，映射字段为positionEstimationConfig_t中的allow_gps_fix_estimation。

2. 设定巡航空速fw_reference_airspeed

测量方法：进行试飞，在 OSD 上开启地速（ground speed）显示，在 CRUISE 模式下朝两个相反方向各飞一段，取两段地速的平均值作为巡航空速。

单位是cm/s。km/h 换算为 cm/s 时乘以 27.77。

示例：100 km/h = 100 × 27.77 = 2777 cm/s

set fw_reference_airspeed=2777

注意两点：

• 飞机在 CRUISE 模式下必须以该设定速度飞行；如果开启了 "Increase cruise speed with throttle"（油门提高巡航速度），在无 GPS Fix 估算期间不要使用它；
• 若安装了皮托，估算会直接使用皮托数据而非该常值，可以不设置fw_reference_airspeed；但官方仍建议设置，作为皮托故障时的后备。

3. 相关命令

若要允许航点任务在无 RC 信号时继续执行（而不是触发 failsafe），可参考命令：

set failsafe_mission_delay=-1

4. 保存设置

执行 CLI 命令后务必保存：

save

四、允许航点任务在估算模式下继续：failsafe_gps_fix_estimation_delay

failsafe_gps_fix_estimation_delay

控制航点任务（waypoint mission）是否允许在 GPS Fix 估算状态下继续。它设置"丢失 GPS fix 事件"与"RTH 激活"之间的时间延迟（秒）：

无论在 configurator 中选择哪种 failsafe 流程，该 RTH 都会被触发。该参数在 src/main/fc/settings.yaml 中定义：默认7，范围-1~600，条件同样是USE_GPS_FIX_ESTIMATION。

五、无磁力计时的表现与误差预期

没有磁力计时导航精度很差，根本问题是航向漂移：飞机在无磁力计、无 GPS 状态下飞得越久，航向估算误差越大。几分钟、几次转弯之后，"北"方向的估算可能完全失效。

总体评价：精度足够在 RC 和 GPS 双丢失时完成 RTH 掉头（U-turn），并在 GPS 偶发中断的区域大致保持 RTH 方向。官方建议仅把无磁力计的 GPS Fix Estimation 当作最后手段——例如飞机飞在湖面上方、降落即丢机时，有估算功能会让飞机朝大致正确的方向尝试 RTH，而不是原地降落。飞走（fly-away）的风险由用户自行评估。

有磁力计 + 气压计时的实测误差

现实的预期误差约为每飞行 1 km 漂移至多 200 m；官方测试中出现过5 km 航程漂移 500 m的情况。

减小漂移的实操建议：

• 起飞后先带 GPS 飞一个大圈，让风估算拿到好的数据；
• 使用空速传感器；若未安装，就保持 CRUISE 模式飞行且不做油门覆盖；
• 转弯要平滑、半径要大；
• 确保罗盘安装方向精确指向机头；
• 正确校准罗盘。

官方视频测试（风为西南向 10 km/h）中的 OSD 观察要点：紫色线为估算位置、黑线为真实位置；"EST ERR" 传感器以米显示估算误差；估算运行期间卫星图标显示 "ES"；GPS 重新定位后估算位置会跳回真实位置。

六、从遥控器禁用 GPS 传感器 Fix（测试方法）

出于测试目的，可以在 Configurator 的 Programming 页签中通过逻辑条件从遥控器禁用 GPS 传感器 fix（即上图所示的编程项）。限制条件：

• 只能在 1) 已获得初始 GPS 定点，且 2) 处于 ARMED（解锁）状态下禁用 GPS。

从源码看，Programming 框架中的LOGIC_CONDITION_DISABLE_GPS_FIX（src/main/programming/logic_condition.c）会置位全局标志LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX；src/main/io/gps.c 的processDisableGPSFix()检测到该标志后，将gpsSol置为无 fix 状态（GPS_NO_FIX、HDOP 9999、卫星数 0）并冻结经纬度坐标。下一次数据处理周期中，updateEstimatedGPSFix()便会接管，生成numSat = 99的估算解——这就是"在真实飞行中人为模拟丢星"的完整链路。

七、为什么多旋翼暂不支持

官方的解释是：对多旋翼无法像固定翼那样做出速度假设（固定翼可假设"朝机头方向、以巡航空速飞行"，多旋翼的速度方向与大小都由桨速/姿态决定且随时可为零），目前尚没有可行方案，只有思路。

八、小结与检查清单

• 该功能面向固定翼，是"丢星 + 丢 RC"双故障下的保命手段，核心用途是无 GPS 的 RTH；
• 硬依赖：气压计（canEstimateGPSFix()要求SENSOR_BARO健康）+ 起飞前的 GPS 定点与 home 保存；软增强：磁力计（航向）、皮托（空速）；
• 最小配置为两条命令加保存：set inav_allow_gps_fix_estimation=ON、set fw_reference_airspeed=<cm/s>、save；
• 航点任务是否允许估算模式续飞由failsafe_gps_fix_estimation_delay控制（默认 7 秒后强制 RTH）；
• 测试时用 Programming 页签的 Disable GPS Fix 逻辑条件模拟丢星，注意需先定点并解锁；
• 预期误差量级为每公里数百米，飞行习惯（大圈校准风、平滑大转弯、罗盘校准）直接影响实际表现。

更多底层实现可继续深入 src/main/navigation/navigation_pos_estimator.c（位置估计器如何消费估算解）与 src/main/navigation/navigation.c（RTH 状态机与安全校验器）。

• 无人机
• 嵌入式
• 智能硬件