type
date
slug
category
icon
password
文档信息目录第一部分：引言与基础第二部分：架构与设计第三部分：核心机制深入第四部分：实践、总结与展望附录第一部分：引言与基础第1章 引言1.1 背景与动机无人机导航面临的核心挑战为什么选择扩展卡尔曼滤波器（EKF）？1.2 文档目标与读者对象文档定位学习路径建议阅读指南1.3 本文档的创新点第2章 卡尔曼滤波器基础2.1 卡尔曼滤波器核心思想核心循环：预测-更新卡尔曼滤波五大方程2.2 扩展卡尔曼滤波器（EKF）为什么需要”扩展”？EKF的解决方案：局部线性化2.3 ArduPilot中的EKF应用EKF在导航系统中的位置2.4 本文档组织结构第2章 卡尔曼滤波器基础2.1 状态估计问题什么是状态？为什么需要估计？2.2 卡尔曼滤波器工作原理（10分钟速成）直观理解：两个朋友的建议卡尔曼增益的物理意义2.3 扩展卡尔曼滤波器（EKF）详解线性化过程示例2.4 EKF vs 互补滤波器第二部分：架构与设计第3章 EKF3架构总览3.1 版本演进历史版本对比3.2 整体架构设计3.2.1 分层架构图3.2.2 类图：代码组织结构 🔍 新增3.3 源码目录结构3.3 数据流向分析3.4 关键组件说明NavEKF3（前端类）NavEKF3_core（后端类）3.5 AHRS集成第4章 状态向量与协方差设计4.1 24维状态向量完整剖析状态分组与用途4.2 为什么是24维？设计原则分析4.2.1 最小参数集原则4.2.2 状态的可观测性分析4.2.3 与传感器的映射关系4.3 24×24协方差矩阵P协方差矩阵的物理意义协方差的演化4.4 为什么选择四元数表示姿态？第三部分：核心机制深入第5章 融合时间视界机制 ⭐5.1 问题提出：传感器延迟不一致挑战：如何融合不同延迟的传感器？5.2 融合时间视界设计5.2.1 核心思想：回溯到过去融合5.2.2 传感器延迟如何确定？ 🔍 深度问题15.2.3 缓冲区大小计算5.3 环形缓冲区架构5.3.1 为什么需要独立缓冲区？ 🔍 深度问题25.3.2 element_t包装结构详解 🔍 深化问题15.3.3 实际传感器数据类型5.4 环形缓冲区操作详解5.4.1 两种缓冲区类型对比5.4.2 recall()函数搜索逻辑 🔍 深度问题3优化后的搜索规则可视化：5.5 时间戳对齐实现5.5.1 传感器数据写入（带时间戳）5.6 融合时刻的确定5.7 输出观测器：补偿延迟到实时5.7.1 输出观测器原理5.7.2 源码实现5.8 融合时间视界总结第6章 EKF主循环与更新机制6.1 UpdateFilter主循环6.2 IMU数据处理与降采样 🔍 深化问题2：runUpdates间隔6.2.1 为什么需要降采样？6.2.2 降采样实现6.2.3 runUpdates触发时间详解6.3 预测步骤详解6.3.1 状态预测：UpdateStrapdownEquationsNED()6.3.2 协方差预测6.4 更新步骤详解6.4.1 序贯融合策略6.4.2 卡尔曼增益计算6.5 融合时间视界机制总结第7章 传感器融合详解7.1 传感器融合总览7.2 磁力计融合（SelectMagFusion）7.2.1 磁力计融合目标7.2.2 测量方程7.2.3 多磁力计自动切换7.3 GPS融合（SelectVelPosFusion）7.3.1 GPS融合目标7.3.2 GPS质量检查（8项）7.3.2 GPS质量检查（8项）7.3.3 动态噪声缩放7.4 高度融合（多源切换）7.5 光流融合（无GPS导航）7.5.1 光流原理7.5.2 光流限制7.6 传感器融合汇总第8章 鲁棒性保障机制 ⭐8.1 创新序列一致性检查8.1.1 创新序列定义8.1.2 卡方检验（Chi-square Test）8.2 传感器故障检测与处理 ⭐⭐⭐ 🔍 深度问题38.2.1 故障检测5层机制8.2.2 GPS断线完整时序分析8.2.3 不同传感器断线影响8.3 超时机制8.4 状态与协方差约束8.4.1 状态约束（ConstrainStates）8.4.2 协方差约束（ConstrainVariances）第9章 多核心冗余与自适应9.1 多核心冗余架构9.2 核心选择机制9.3 导航模式自动切换9.4 自适应噪声缩放第10章 参数配置与调优实践10.1 核心参数列表10.1.1 必须了解的参数10.1.2 传感器噪声参数（可调）10.1.3 危险参数（不建议修改）10.2 不同场景配置指南场景1：室外GPS飞行（默认配置）场景2：室内光流飞行场景3：GPS拒止环境（自动降级）10.3 参数调优流程第11章 问题诊断与案例11.1 日志分析方法11.1.1 关键日志消息11.2 典型问题诊断问题1：GPS创新序列过大问题2：磁力计创新序列过大11.3 案例研究案例1：城市峡谷GPS多路径第12章 总结与展望12.1 EKF3核心技术总结10大核心机制12.2 关键设计理念12.3 使用建议12.4 EKF3的核心优势12.5 进阶学习资源12.6 未来展望12.7 结语附录附录A：参数完整列表A.1 基础参数A.2 噪声参数A.3 创新门限附录B：源码索引B.1 关键函数索引B.2 重要数据结构附录C：术语表
文档信息
- 版本: v1.0
- 创建日期: 2024年
- 适用版本: ArduPilot Copter/Plane/Rover 3.5+
- 作者: 基于ArduPilot源码分析
- 许可: GPL v3.0
目录
第一部分：引言与基础
- 第1章 引言
- 第2章 卡尔曼滤波器基础
第二部分：架构与设计
- 第3章 EKF3架构总览
- 第4章 状态向量与协方差设计
第三部分：核心机制深入
- 第5章 融合时间视界机制
- 第6章 EKF主循环与更新
- 第7章 传感器融合详解
- 第8章 鲁棒性保障机制
- 第9章 多核心冗余与自适应
第四部分：实践、总结与展望
- 第10章 参数配置与调优
- 第11章 问题诊断与案例
- 第12章 总结与展望
附录
- 附录A：参数完整列表
- 附录B：源码索引
- 附录C：术语表
第一部分：引言与基础
第1章 引言
1.1 背景与动机
ArduPilot是全球最流行的开源自动驾驶仪项目，为多旋翼、固定翼、直升机、地面车辆等提供完整的飞行控制解决方案。在自主导航系统中，准确的状态估计是一切控制决策的基础。
无人机导航面临的核心挑战
为什么选择扩展卡尔曼滤波器（EKF）？
方法 | 优点 | 缺点 | ArduPilot应用 |
互补滤波器 | 计算简单，实时性好 | 无传感器误差估计，精度有限 | DCM备用算法 |
粒子滤波器 | 可处理任意非线性 | 计算量巨大，难以实时 | 不适用 |
扩展卡尔曼滤波器 | 精度高，可估计误差，计算可行 | 线性化误差，需要调参 | ✅ 主要方案 |
无迹卡尔曼滤波器 | 精度更高，无需雅可比 | 计算量较大 | 未采用 |
EKF的核心优势：
- ✅ 最优融合多传感器数据
- ✅ 在线估计传感器误差（陀螺/加速度偏差）
- ✅ 提供不确定性估计（协方差矩阵）
- ✅ 实时性满足要求（100Hz）
- ✅ 成熟稳定，工程验证充分
1.2 文档目标与读者对象
文档定位
本文档是ArduPilot NavEKF3的技术深度分析，基于源码剖析其实现机制，适合：
学习路径建议
阅读指南
- 🔴 必读章节 - 第1, 3, 5, 7, 10章
- 🟡 推荐章节 - 第2, 4, 6, 8, 11章
- 🟢 选读章节 - 第9, 12章及附录
- 💻 源码示例 - 浅灰色背景代码块
- 📊 可视化图表 - Mermaid图表辅助理解
1.3 本文档的创新点
相比现有资料，本文档的独特价值：
- 源码级深度分析 - 不仅讲原理，更剖析实现细节
- 融合时间视界详解 - ArduPilot独特设计的完整揭示
- 传感器断线处理 - 5层防护机制的完整追踪
- 可视化丰富 - 30+张Mermaid图表
- 实战导向 - 包含调优、诊断、案例研究
第2章 卡尔曼滤波器基础
2.1 卡尔曼滤波器核心思想
卡尔曼滤波器是一个递归最优估计器，通过融合传感器测量和系统模型，给出状态的最优估计。
核心循环：预测-更新
卡尔曼滤波五大方程
符号说明：
x̂- 状态估计向量
P- 协方差矩阵(表示估计的不确定性)
F- 状态转移矩阵
B- 控制输入矩阵
u- 控制输入向量
Q- 过程噪声协方差
H- 测量矩阵
z- 测量值
R- 测量噪声协方差
K- 卡尔曼增益(融合权重)
I- 单位矩阵
- 上标
ᵀ- 矩阵转置
- 上标
⁻- 先验估计(预测值)
2.2 扩展卡尔曼滤波器（EKF）
为什么需要”扩展”？
标准卡尔曼滤波器仅适用于线性系统，但飞行器运动是高度非线性的：
EKF的解决方案：局部线性化
代价：
- ✅ 可以处理非线性系统
- ⚠️ 线性化误差（高度非线性时误差大）
- ⚠️ 需要计算雅可比矩阵（计算量增加）
2.3 ArduPilot中的EKF应用
EKF在导航系统中的位置
EKF3提供的关键输出：
- 姿态（Roll, Pitch, Yaw）
- 速度（North, East, Down）
- 位置（Latitude, Longitude, Altitude）
- IMU偏差估计（陀螺仪、加速度计）
- 风速估计
- 磁场估计
这些输出直接驱动飞行控制器！
2.4 本文档组织结构
阅读时间估计：
- 快速浏览：2小时
- 认真学习：8小时
- 深入研究：20+小时（含源码）
第2章 卡尔曼滤波器基础
2.1 状态估计问题
什么是状态？
状态是描述系统当前状况的最小变量集。对于飞行器：
为什么需要估计？
传感器测量不完美：
卡尔曼滤波器的作用：融合多个不完美的测量，给出比任何单一传感器更准确的估计。
2.2 卡尔曼滤波器工作原理（10分钟速成）
直观理解：两个朋友的建议
卡尔曼增益的物理意义
2.3 扩展卡尔曼滤波器（EKF）详解
线性化过程示例
ArduPilot的实现：
- 使用符号数学工具（Matlab）自动推导雅可比矩阵
- 生成优化的C++代码
- 参考：https://github.com/PX4/ecl/blob/master/matlab/scripts/…
2.4 EKF vs 互补滤波器
ArduPilot同时支持EKF和DCM（互补滤波器），对比如下：
特性 | EKF3 | DCM |
精度 | 高（多传感器最优融合） | 中（简单加权） |
传感器误差估计 | ✅ 在线估计偏差 | ❌ 无估计 |
不确定性估计 | ✅ 提供协方差 | ❌ 无 |
计算量 | 高（24×24矩阵） | 低（向量运算） |
内存占用 | 高（~10KB/核心） | 低（~1KB） |
调参难度 | 中（需理解参数） | 低（几乎无需调参） |
故障容错 | 强（多核心冗余） | 弱 |
推荐使用 | ✅ 主要方案 | 备用/降级 |
何时使用DCM？
- CPU性能受限（<150MHz）
- 内存不足
- EKF故障时自动降级
第二部分：架构与设计
第3章 EKF3架构总览
3.1 版本演进历史
ArduPilot的EKF经历了三代演进：
版本对比
特性 | EKF1 | EKF2 | EKF3 |
状态数 | 21-24 | 24-28 | 24 |
核心数 | 1 | 1-3 | 1-3 |
状态 | 已弃用 | 在用 | 推荐 |
加速度偏差 | Z轴 | 增量+尺度 | 3D完整 |
协方差方程 | 手写 | Matlab生成 | Matlab生成+优化 |
代码基础 | priseborough推导 | 同EKF1 | PX4 ECL推导 |
主要改进 | - | 多核心 | 更好数值稳定性 |
本文档聚焦EKF3（当前推荐版本）
3.2 整体架构设计
3.2.1 分层架构图
3.2.2 类图：代码组织结构 🔍 新增
类的职责划分：
- NavEKF3（前端）
- 管理多个EKF核心
- 参数配置与存储
- 核心选择与切换
- 提供统一接口给AHRS
- NavEKF3_core（后端）
- 实际的EKF算法实现
- 每个IMU独立实例
- 状态预测与更新
- 传感器数据融合
- 环形缓冲区
- 时序管理
- 数据存储与检索
- 模板化设计，类型安全
3.3 源码目录结构
设计模式：
- 前端-后端分离：NavEKF3（前端）管理多个NavEKF3_core（后端）
- 模板化缓冲区：类型安全，代码复用
- 功能模块化：每个传感器独立文件，易于维护
3.3 数据流向分析
3.4 关键组件说明
NavEKF3（前端类）
职责：
- 管理多个EKF核心
- 存储所有参数
- 选择最优核心
- 提供统一接口
NavEKF3_core（后端类）
职责：
- 实现完整的EKF算法
- 管理状态和协方差
- 管理传感器缓冲区
- 执行预测和更新
3.5 AHRS集成
ArduPilot使用AHRS（姿态航向参考系统）作为导航接口层：
EKF类型选择：
第4章 状态向量与协方差设计
4.1 24维状态向量完整剖析
状态分组与用途
4.2 为什么是24维？设计原则分析
4.2.1 最小参数集原则
关键问题：为什么不包含传感器原始数据？
4.2.2 状态的可观测性分析
不是所有状态都能被准确估计，可观测性取决于传感器配置：
状态 | 观测性 | 依赖传感器 | 备注 |
姿态roll/pitch | ⭐⭐⭐⭐⭐ | 加速度计 | 重力向量 |
姿态yaw | ⭐⭐⭐⭐ | 磁力计或GPS航向 | 无磁力计时较差 |
水平速度 | ⭐⭐⭐⭐⭐ | GPS或光流 | GPS失效时依赖光流 |
垂直速度 | ⭐⭐⭐⭐ | GPS或气压计 | 气压计提供高度变化率 |
水平位置 | ⭐⭐⭐⭐⭐ | GPS或光流 | 绝对位置需GPS |
垂直位置 | ⭐⭐⭐⭐⭐ | 气压计/GPS/测距 | 多源融合 |
陀螺偏差 | ⭐⭐⭐⭐ | GPS/磁力计/重力 | 需要外部姿态参考 |
加速度偏差 | ⭐⭐⭐ | GPS速度 | 长时间飞行收敛 |
地磁场 | ⭐⭐⭐⭐ | 磁力计+GPS | 需要机动激励 |
机体磁场 | ⭐⭐⭐ | 磁力计+机动 | 需要多方向飞行 |
风速 | ⭐⭐⭐ | 空速计+GPS | 需要空速计 |
可观测性影响调参：
- 可观测性差的状态，初始方差设大，过程噪声设大
- 可观测性好的状态，初始方差设小，过程噪声设小
4.2.3 与传感器的映射关系
关键理解：
- 状态向量是隐变量，不能直接测量
- 传感器测量是状态的函数（通过测量方程H）
- 卡尔曼滤波器通过测量反推状态
4.3 24×24协方差矩阵P
协方差矩阵的物理意义
协方差的演化
协方差的作用：
1. 指导卡尔曼增益：P大→K大→更信任测量
2. 健康监控：P过大→状态不确定→可能故障
3. 创新检查：预测方差用于判断测量是否异常
4.4 为什么选择四元数表示姿态？
万向节锁演示：
第三部分：核心机制深入
第5章 融合时间视界机制 ⭐
5.1 问题提出：传感器延迟不一致
挑战：如何融合不同延迟的传感器？
如果不处理延迟会怎样？
5.2 融合时间视界设计
5.2.1 核心思想：回溯到过去融合
关键设计决策：
- 融合时刻 = IMU缓冲区最旧数据时刻
- 为什么？确保GPS等慢速传感器数据已到达
- 延迟补偿：通过输出观测器前向传播到当前时刻
5.2.2 传感器延迟如何确定？ 🔍 深度问题1
方法1：固定参数（编译时常量）
方法2：可配置参数（用户可调）
方法3：动态获取（GPS特殊处理）
为什么GPS延迟是动态的?
- 不同GPS模块延迟不同（u-blox: 120ms, NMEA: 200ms）
- GPS驱动最了解自己的延迟
- 自适应不同硬件配置
5.2.3 缓冲区大小计算
计算示例：
5.3 环形缓冲区架构
5.3.1 为什么需要独立缓冲区？ 🔍 深度问题2
答案：每个传感器类型有独立缓冲区，实现故障隔离
独立缓冲区的优势：
关键特性：单个传感器故障不影响其他传感器融合！
5.3.2 element_t包装结构详解 🔍 深化问题1
为什么需要element_t包装？
内存布局可视化：
5.3.3 实际传感器数据类型
所有数据类型都包含time_ms时间戳！ 这是时间对齐的基础。
5.4 环形缓冲区操作详解
5.4.1 两种缓冲区类型对比
类型1：IMU缓冲区（高频，简单FIFO）
特点：
- 简单FIFO（先进先出）
- 不需要时间戳搜索
- 适合高频连续数据
类型2：观测缓冲区（低频，时间戳检索）
特点：
- 基于时间戳检索
- 自动找最接近fusion_time的数据
- 拒绝过时数据（>100ms）
- 防止数据重复使用（清零时间戳）
5.4.2 recall()函数搜索逻辑 🔍 深度问题3
你选中的这段代码逻辑确实存在问题。让我来优化这个搜索规则的示例：
优化后的搜索规则可视化：
搜索规则总结：
5.5 时间戳对齐实现
5.5.1 传感器数据写入（带时间戳）
时间戳计算细节：
5.6 融合时刻的确定
为什么融合时间 = 最旧IMU时刻？
5.7 输出观测器：补偿延迟到实时
融合发生在t-150ms，但飞控需要t=0ms的估计！
5.7.1 输出观测器原理
5.7.2 源码实现
时间常数EK3_TAU_OUTPUT的影响：
5.8 融合时间视界总结
关键要点：
1. ✅ 融合时刻 = 最旧IMU时刻（确保数据完整）
2. ✅ 每个传感器独立缓冲区（故障隔离）
3. ✅ 基于时间戳检索（精确对齐）
4. ✅ 输出观测器补偿延迟（实时输出）
5. ✅ 不是简单的时间戳对齐，而是完整的延迟补偿系统！
第6章 EKF主循环与更新机制
6.1 UpdateFilter主循环
流程图：
6.2 IMU数据处理与降采样 🔍 深化问题2：runUpdates间隔
6.2.1 为什么需要降采样？
6.2.2 降采样实现
6.2.3 runUpdates触发时间详解
不同IMU频率的实际表现：
IMU频率 | dtIMUavg | 累加帧数 | 实际间隔 | EKF更新率 |
1000 Hz | 1.0 ms | 10帧 | 10 ms | 100 Hz |
800 Hz | 1.25 ms | 8帧 | 10 ms | 100 Hz |
400 Hz | 2.5 ms | 4帧 | 10 ms | 100 Hz |
200 Hz | 5.0 ms | 2帧 | 10 ms | 100 Hz |
100 Hz | 10 ms | 1帧 | 10 ms | 100 Hz |
50 Hz | 20 ms | 1帧 | 20 ms | 50 Hz (跟随IMU) |
结论：runUpdates间隔是10ms（100Hz），不是50ms！
6.3 预测步骤详解
6.3.1 状态预测：UpdateStrapdownEquationsNED()
为什么使用梯形积分？
6.3.2 协方差预测
过程噪声Q矩阵：
6.4 更新步骤详解
6.4.1 序贯融合策略
GPS提供6个测量（3速度+3位置），为什么不一次融合？
ArduPilot采用序贯融合：
6.4.2 卡尔曼增益计算
6.5 融合时间视界机制总结
融合时间视界的精髓：
- 不在当前时刻融合（数据未齐）
- 回溯到过去融合（数据已齐）
- 通过观测器前向传播（补偿延迟）
- 用户获得实时估计（无感知延迟）
第7章 传感器融合详解
7.1 传感器融合总览
融合模块对比表：
融合模块 | 输入测量 | 更新状态 | 主要目标 | 更新频率 |
SelectMagFusion | 磁场XYZ | quat, earth_mag, body_mag | 航向校正 | ~75Hz |
SelectVelPosFusion | GPS速度+位置 | velocity, position | 绝对导航 | 5-10Hz |
selectHeightForFusion | BARO/RNG/GPS | position.z | 高度估计 | 50Hz |
SelectFlowFusion | 光流角速率 | velocity, terrain | 室内导航 | 10-20Hz |
SelectTasFusion | 空速 | velocity, wind_vel | 风速估计 | 10Hz |
SelectBetaFusion | 侧滑=0 | velocity.y | 固定翼约束 | 25Hz |
7.2 磁力计融合（SelectMagFusion）
7.2.1 磁力计融合目标
7.2.2 测量方程
更新的状态：
- quat（特别是yaw分量）
- earth_magfield（地磁场估计）
- body_magfield（机体磁干扰）
7.2.3 多磁力计自动切换
不是融合多个磁力计的平均值！ 而是选择最健康的一个。
7.3 GPS融合（SelectVelPosFusion）
7.3.1 GPS融合目标
7.3.2 GPS质量检查（8项）
7.3.2 GPS质量检查（8项）
7.3.3 动态噪声缩放
自适应效果：
7.4 高度融合（多源切换）
高度源对比：
数据源 | 精度 | 优点 | 缺点 | 适用场景 |
测距仪 | 0.1m | 高精度，无漂移 | 需稳定地面，高度受限 | 低空(<5m) |
气压计 | 1-2m | 无漂移，响应快 | 受气流影响 | 正常飞行 |
GPS | 3-5m | 绝对参考 | 精度较差 | 气压计故障时 |
7.5 光流融合（无GPS导航）
7.5.1 光流原理
7.5.2 光流限制
7.6 传感器融合汇总
关键特性：
- 各传感器融合独立（recall失败不影响其他）
- 序贯融合（逐个处理，降低计算量）
- 条件融合（数据可用且健康才融合）
第8章 鲁棒性保障机制 ⭐
8.1 创新序列一致性检查
8.1.1 创新序列定义
8.1.2 卡方检验（Chi-square Test）
3σ原则：
8.2 传感器故障检测与处理 ⭐⭐⭐ 🔍 深度问题3
8.2.1 故障检测5层机制
8.2.2 GPS断线完整时序分析
关键保障：
- ✅ GPS断线不影响MAG/BARO融合（独立缓冲区）
- ✅ 渐进式降级（正常→旧数据→跳过→超时→切换）
- ✅ 自动恢复（GPS恢复后重新使用）
- ⚠️ 恢复时可能跳变（需要重置位置）
8.2.3 不同传感器断线影响
断线传感器 | 短期影响(0-5s) | 长期影响(>10s) | 降级方案 | 飞行安全性 |
GPS | 位置P增大 | 位置漂移 | 光流/纯IMU | ✅ 可短时飞行⚠️ 建议返航 |
磁力计 | 航向P增大 | 航向漂移 | GPS航向/惯性 | ✅ 固定翼可飞⚠️ 多旋翼风险 |
气压计 | 高度P增大 | 高度漂移 | GPS/测距仪 | ✅ 有备用源❌ 无备用危险 |
测距仪 | 低空高度差 | 光流受影响 | 气压计 | ✅ 影响小 |
光流 | 室内失控 | 无GPS时无法导航 | GPS | ⚠️ 看场景 |
GPS+MAG | 姿态/位置双失效 | 快速漂移 | 无 | ❌ 紧急降落 |
8.3 超时机制
超时时间常数表：
传感器 | 超时时间 | 参数名 | 说明 |
GPS速度/位置 | 7秒 | posRetryTimeUseVel_ms | 有速度辅助 |
GPS位置（无速度） | 10秒 | posRetryTimeNoVel_ms | 无速度辅助 |
磁力计 | 10秒 | magFailTimeLimit_ms | 足够长避免误报 |
高度（有GPS垂直速度） | 5秒 | hgtRetryTimeMode0_ms | 较短 |
高度（无GPS垂直速度） | 10秒 | hgtRetryTimeMode12_ms | 较长 |
空速 | 10秒 | tasRetryTime_ms | 固定翼专用 |
光流 | 5秒 | flowIntervalMax_ms | 室内导航 |
8.4 状态与协方差约束
8.4.1 状态约束（ConstrainStates）
为什么需要约束？
- 数值不稳定可能导致状态爆炸
- 传感器故障可能导致异常估计
- 约束确保物理合理性
8.4.2 协方差约束（ConstrainVariances）
方差上限的物理意义：
第9章 多核心冗余与自适应
9.1 多核心冗余架构
多核心优势：
- IMU0故障 → 自动切换到IMU1
- 提供冗余，提高可靠性
- 每个核心独立运行，互不干扰
9.2 核心选择机制
errorScore()计算：
9.3 导航模式自动切换
9.4 自适应噪声缩放
自适应效果图：
第10章 参数配置与调优实践
10.1 核心参数列表
10.1.1 必须了解的参数
10.1.2 传感器噪声参数（可调）
10.1.3 危险参数（不建议修改）
10.2 不同场景配置指南
场景1：室外GPS飞行（默认配置）
场景2：室内光流飞行
场景3：GPS拒止环境（自动降级）
10.3 参数调优流程
调优黄金规则：
1. 硬件第一，参数第二
2. 每次只改一个参数
3. 改后必须测试验证
4. 记录日志对比前后
第11章 问题诊断与案例
11.1 日志分析方法
11.1.1 关键日志消息
11.2 典型问题诊断
问题1：GPS创新序列过大
问题2：磁力计创新序列过大
11.3 案例研究
案例1：城市峡谷GPS多路径
第12章 总结与展望
12.1 EKF3核心技术总结
10大核心机制
# | 机制 | 核心价值 | 实现亮点 |
1 | 融合时间视界 | 解决传感器延迟不一致 | 回溯融合+观测器补偿 |
2 | 独立环形缓冲区 | 故障隔离 | 每个传感器独立，断线不影响其他 |
3 | runUpdates机制 | IMU降采样 | 高频降到100Hz，避免锥运动误差 |
4 | 创新序列检查 | 异常数据拒绝 | 3σ门限，卡方检验 |
5 | 超时检测 | 传感器故障检测 | 多级超时，渐进式降级 |
6 | 模式自动切换 | 适应传感器可用性 | GPS/光流/纯IMU自动切换 |
7 | 多核心冗余 | IMU故障容错 | 自动选择最健康核心 |
8 | 动态噪声缩放 | 自适应传感器质量 | GPS精度差时降权 |
9 | 状态/协方差约束 | 防止发散 | 物理上下限，数值稳定 |
10 | 磁场在线学习 | 补偿磁干扰 | 实时估计机体磁场 |
12.2 关键设计理念
12.3 使用建议
12.4 EKF3的核心优势
12.5 进阶学习资源
12.6 未来展望
12.7 结语
ArduPilot的NavEKF3是一个工程级的卡尔曼滤波器实现典范：
- ✅ 理论扎实 - 基于Paul Riseborough的严格数学推导
- ✅ 工程优秀 - 考虑了实时性、鲁棒性、可维护性
- ✅ 久经考验 - 数百万飞行小时验证
- ✅ 持续演进 - 活跃的开源社区
最重要的一句话：
EKF是自适应的智能系统，它会自动处理大部分情况。你的任务是提供优质的传感器数据，而不是过度调参！
祝您的飞行器飞得又稳又准！ 🚁✈️
附录
附录A：参数完整列表
A.1 基础参数
参数 | 默认值 | 范围 | 说明 |
EK3_ENABLE | 1 | 0-1 | 启用EKF3 |
EK3_IMU_MASK | 3 | 1-7 | IMU位掩码(1=IMU0, 2=IMU1, 4=IMU2) |
EK3_GPS_TYPE | 0 | 0-3 | 0=3D, 1=2D, 2=仅位置, 3=禁用 |
EK3_ALT_SOURCE | 0 | 0-2 | 0=BARO, 1=RNG, 2=GPS |
EK3_MAG_CAL | 1 | 0-4 | 磁场学习模式 |
A.2 噪声参数
参数 | 默认值 | 单位 | 说明 |
EK3_VELNE_M_NSE | 0.3 | m/s | GPS水平速度噪声 |
EK3_VELD_M_NSE | 0.5 | m/s | GPS垂直速度噪声 |
EK3_POSNE_M_NSE | 1.0 | m | GPS水平位置噪声 |
EK3_ALT_M_NSE | 2.0 | m | 气压计噪声 |
EK3_MAG_M_NSE | 0.05 | Gauss | 磁力计噪声 |
EK3_RNG_M_NSE | 0.5 | m | 测距仪噪声 |
EK3_FLOW_M_NSE | 0.25 | rad/s | 光流噪声 |
A.3 创新门限
参数 | 默认值 | 说明 |
EK3_VELNE_I_GATE | 300 | GPS速度创新门限(%) → 3σ |
EK3_POSNE_I_GATE | 300 | GPS位置创新门限(%) → 3σ |
EK3_ALT_I_GATE | 300 | 高度创新门限(%) → 3σ |
EK3_MAG_I_GATE | 300 | 磁力计创新门限(%) → 3σ |
附录B：源码索引
B.1 关键函数索引
函数名 | 文件 | 行号 | 功能 |
UpdateFilter() | AP_NavEKF3_core.cpp | 506 | 主更新循环 |
readIMUData() | AP_NavEKF3_Measurements.cpp | 277 | IMU降采样 |
UpdateStrapdownEquationsNED() | AP_NavEKF3_core.cpp | 588 | 状态预测 |
CovariancePrediction() | AP_NavEKF3_core.cpp | 805 | 协方差预测 |
SelectMagFusion() | AP_NavEKF3_MagFusion.cpp | 218 | 磁力计融合 |
SelectVelPosFusion() | AP_NavEKF3_PosVelFusion.cpp | 227 | GPS融合 |
calcOutputStates() | AP_NavEKF3_core.cpp | 661 | 输出观测器 |
setAidingMode() | AP_NavEKF3_Control.cpp | 178 | 模式切换 |
B.2 重要数据结构
结构名 | 文件 | 行号 | 说明 |
state_elements | AP_NavEKF3_core.h | 378 | 24维状态向量 |
imu_elements | AP_NavEKF3_core.h | 395 | IMU数据 |
gps_elements | AP_NavEKF3_core.h | 403 | GPS数据 |
mag_elements | AP_NavEKF3_core.h | 411 | 磁力计数据 |
obs_ring_buffer_t | AP_NavEKF3_Buffer.h | 7 | 观测缓冲区 |
imu_ring_buffer_t | AP_NavEKF3_Buffer.h | 118 | IMU缓冲区 |
附录C：术语表
术语 | 英文 | 说明 |
扩展卡尔曼滤波器 | Extended Kalman Filter | EKF |
融合时间视界 | Fusion Time Horizon | 回溯到过去的融合时刻 |
创新序列 | Innovation | 测量值-预测值 |
协方差 | Covariance | 不确定性度量 |
卡尔曼增益 | Kalman Gain | 融合权重 |
状态向量 | State Vector | 被估计的变量集合 |
测量方程 | Measurement Equation | 传感器与状态的关系 |
序贯融合 | Sequential Fusion | 逐个融合测量 |
雅可比矩阵 | Jacobian Matrix | 偏导数矩阵 |
NED坐标系 | North-East-Down | 北东地坐标系 |
文档完成！ 🎉
总页数: 约120页
总字数: 约4万字
图表数: 30+张
代码示例: 50+个
- Author:felixfixit
- URL:http://www.felixmicrospace.top/article/apm_ekf_docs
- Copyright:All articles in this blog, except for special statements, adopt BY-NC-SA agreement. Please indicate the source!
