- 博客(69)
- 收藏
- 关注
RSS订阅原创 PX4 EKF2 源码解析（十九）：空速、侧滑、阻力与风速估计
EKF2 的水平风速状态需要空气动力观测才能稳定估计。固定翼主要使用真空速和零侧滑约束，多旋翼可使用机体阻力模型。本文给出观测模型、可观性和源码结构。
2026-09-13 13:37:04
126
原创 PX4 EKF2 源码解析（十八）：光流、测距与地形高度估计
光流提供图像角运动，不能单独给出米制水平速度；其尺度依赖对地距离。EKF2 将光流、测距仪和独立地形估计器组合，用于无 GNSS 低空定位。本文推导基本观测关系并分析适用边界。
2026-09-13 13:36:15
244
原创 PX4 EKF2 源码解析（十七）：外部视觉、VIO 与坐标系对齐
外部视觉可提供位置、速度、姿态或其组合，但其参考系、时间延迟和重定位行为与 GNSS 明显不同。本文解析到 EKF2 的数据链，以及位置、速度、航向和高度四类融合控制。
2026-08-31 12:50:59
111
原创 PX4 EKF2 源码解析（十六）：气压、GNSS、测距与视觉高度融合
垂直通道同时受到重力投影、气压变化、地形变化和振动影响。EKF2 将一个高度源设为垂直参考，其余来源通过偏差估计辅助或备份。本文分析高度符号、各观测源和故障回退。
2026-08-31 12:49:55
120
原创 PX4 EKF2 源码解析（十五）：磁力计、GNSS 航向与偏航可观性
偏航绕重力轴旋转，不能由静止加速度计确定。EKF2 可使用磁力计、双天线 GNSS、外部视觉及运动速度信息建立航向。本文重点解析磁航向、三轴磁融合、磁场状态和 GNSS 航向融合。
2026-08-30 17:34:59
17
原创 PX4 EKF2 源码解析（十四）：GNSS 位置与速度融合
GNSS 是 EKF2 最主要的全局水平辅助源。本文分析 GNSS 质量检查、本地坐标转换、位置与速度观测模型、噪声约束、融合状态机及失效恢复。
2026-08-30 17:34:14
21
原创 PX4 EKF2 源码解析（十三）：`controlFusionModes()` 与观测融合状态机
观测模型只能回答“如何融合”，不能回答“此刻是否应该融合”。EKF2 为每类观测设置启动、持续、停止和重置条件，并由统一调度。本文提炼各共有的状态机模式。它位于真实观测和伪观测控制之后，因此会看到本周期最新。若把该调用提前，刚成功融合的观测可能要下一周期才清除 dead-reckoning flag。
2026-08-28 12:47:14
322
原创 PX4 EKF2 源码解析（十二）：Output Predictor 的低延迟状态重建
Output Predictor 把延迟 EKF 的姿态、速度和位置恢复到最新 IMU 时刻，并以受控方式吸收主滤波器修正。本文解析其惯性积分、误差反馈、时间常数和状态重置同步机制。
2026-08-28 12:46:39
151
原创 PX4 EKF2 源码解析（十一）：延迟融合时域与当前输出时域
PX4 EKF2 主滤波器不在最新 IMU 时刻融合观测，而是在一段时间之前的 delayed horizon 工作；控制器使用的状态则位于 current/output horizon。本文用时间轴说明两个时域的建立及数据一致性。
2026-08-27 12:38:34
268
原创 PX4 EKF2 源码解析（十）：IMU 降采样、RingBuffer 与测量延迟
EKF2 必须同时处理数百赫兹 IMU 和数赫兹至数十赫兹辅助传感器。本文解析RingBuffer和各传感器时间戳修正，说明数据“何时到达”和“测量属于何时”之间的区别。
2026-08-27 12:37:36
141
原创 PX4 EKF2 源码解析（九）：通用观测更新内核 `measurementUpdate()
不同传感器具有不同观测模型，但最终都要完成协方差和状态更新。本文以标量观测为对象，解析创新方差、卡尔曼增益、状态抑制、协方差健康检查和fuse()，建立各的统一阅读模板。
2026-08-26 12:42:11
126
原创 PX4 EKF2 源码解析（八）：协方差预测、过程噪声与数值稳定性
状态预测给出下一时刻的名义状态，协方差预测描述该结果可能有多大误差。本文解释 EKF2 的线性化传播、IMU 噪声和状态随机游走，并分析生成代码、协方差对称性及非负方差保护。
2026-08-26 12:41:05
17
原创 PX4 EKF2 源码解析（七）：`predictState()` 与惯性导航递推
状态预测把一段时间内的角增量和速度增量传播到姿态、速度与位置。本文逐项推导 PX4 v1.14.3，说明偏差补偿、地球自转、坐标旋转、重力补偿和梯形位置积分的实现。
2026-08-24 12:43:51
24
原创 PX4 EKF2 源码解析（六）：滤波器初始化与姿态对准
EKF 启动并不等于立即获得完整导航解。PX4 EKF2 先检查 IMU 积分周期，低通积累静止数据，以重力方向初始化倾斜角，再建立协方差和输出预测器；偏航与水平辅助则在后续融合状态机中完成。本文解析初始化条件及各状态标志的时序。
2026-08-24 12:43:03
141
原创 PX4 EKF2 源码解析（五）：24 维状态向量、坐标系与四元数
结构体本身不保证内存布局可直接转成Vector24f；矩阵运算仍使用明确切片。理解索引应以fuse()和生成脚本为共同依据。
2026-08-23 11:37:45
18
原创 PX4 EKF2 源码解析（四）：从卡尔曼滤波到非线性状态估计
本文建立阅读 EKF2 源码所需的最小数学框架，说明状态预测、协方差预测、创新、卡尔曼增益和观测更新的关系，并特别解释 PX4 创新符号与常见教材定义相反时，源码中的减法为何仍与标准卡尔曼更新一致。
2026-08-23 11:36:51
35
原创 PX4 EKF2 源码解析（三）：`EKF2::Run()` 主循环与完整数据链
是 PX4 接口层的运行入口。它将工作队列、uORB、传感器样本、主 EKF 与输出发布连接起来。本文沿一帧 IMU 数据的生命周期分析主循环，明确高频路径、滤波更新路径和低频诊断路径。关键词：PX4、EKF2、uORB、ScheduledWorkItem、实时数据流。
2026-08-23 11:35:48
34
原创 PX4 EKF2 源码解析（二）：接口层、算法层与符号生成层
EKF2 的复杂性不仅来自滤波方程，还来自实时数据接口、传感器时间对齐、功能裁剪和自动生成代码。本文按照职责将源码划分为 PX4 接口层、EKF 算法层和符号生成层，并给出对象关系、数据流与推荐阅读顺序。关键词：PX4、EKF2、软件架构、EstimatorInterface、SymForce。
2026-08-23 11:34:44
298
原创 PX4 EKF2 源码解析（一）：状态估计问题与 EKF2 的工程定位
飞行控制器不能直接把陀螺仪、加速度计、GNSS、磁力计和气压计读数作为控制状态。各传感器的采样频率、延迟、噪声模型和可观测量不同，必须由状态估计器在统一的动力学模型中完成时间对齐、信息融合与故障隔离。本文从飞行器状态估计问题出发，说明 PX4 EKF2 的输入、输出、设计目标以及它在飞控软件栈中的边界。关键词：PX4、EKF2、状态估计、传感器融合、飞行控制。
2026-08-23 11:33:17
663
原创 FAST-LIVO2 源码精读（十八）：实时性优化、漂移排查与工程落地
FAST-LIVO2 的工程价值，不在于某一个孤立的技巧，而在于它把"激光的几何精度、惯性的高频预测、视觉的纹理与色彩"用一套简洁的滤波框架紧紧拧在一起，又在实时性、鲁棒性、可扩展性之间取得了难得的平衡。先用内建计时表定位瓶颈，再按降采样、并行、滑窗、编译优化、双频输出逐项加速；遇到漂移先看现象、循诊断树定位根因、回到对应章节取解法。十八篇至此收束。
2026-06-25 12:47:21
214
原创 FAST-LIVO2 源码精读（十七）：建图保存、COLMAP 对接与全局一致性
本文是「FAST-LIVO2 激光-惯性-视觉里程计源码精读」专栏第十七篇。上一篇打通了从官方数据集到自有硬件的接入路径，把雷达适配、相机模型与两组外参的标定流程逐一落到 YAML。系统能稳定运行之后，下一个工程问题随之而来：跑完一段数据，建图结果如何落盘、彩色点云怎样导出、能不能对接 COLMAP 做三维重建——以及一个绕不开的事实，FAST-LIVO2 是纯里程计、不带回环，长距离下首尾必然对不齐，全局一致性该如何补上。本篇锚定scripts/与local_map的真实代码逐一回答。
2026-06-25 12:46:40
483
原创 FAST-LIVO2 源码精读（十六）：接入自定义传感器——多雷达适配与内外参标定
本文是「FAST-LIVO2 激光-惯性-视觉里程计源码精读」专栏第十六篇。上一篇拆解了中每个关键参数的物理含义与调参路径，并反复强调外参RclPcl与时间偏移配错即全局失效；本篇承接这一悬念，回答更前置的工程问题——如何把 FAST-LIVO2 接到一套全新硬件上：新雷达如何通过的类型分支适配、相机内参与畸变模型如何配置、两组外参与时间偏移如何标定出来并填入 YAML，最后给出一份落地检查清单。
2026-06-25 12:46:15
197
原创 FAST-LIVO2 源码精读（十五）：关键参数与外参调优
FAST-LIVO2 的官方仓库里， 目录下并排放着 、、 等多份配置。它们驱动的是完全相同的一份可执行文件，差别只在参数。把 直接套到 Hilti 数据集上，结果往往是点云错层、轨迹漂移；反之亦然。差异藏在一些不起眼的数字里： 的 ，而 是 ； 的 ， 是 。这些值并非随手填写，每一个都对应着一个明确的物理量——传感器噪声、平面判定的严格程度、像素量测的可信度。理解它们的含义，是把 FAST-LIVO2 从"能跑通官方数据集"推进到"能跑通自己设备"的必经一步。更值得警惕的是：YAML 里写了的参数，
2026-06-25 12:45:41
76
原创 FAST-LIVO2 源码精读（十四）：雷达-视觉顺序紧耦合与单一 ESIKF 状态融合
EKF_STATE是挂在上的标志位，指示下一次应执行哪条分支。SLAM_MODEONLY_LIO：仅做雷达惯性里程计，切割时序与纯 LIO 系统相同；LIVO：雷达+视觉+惯性，进入顺序更新逻辑；ONLY_LO：无 IMU，仅雷达里程计。
2026-06-24 12:50:13
148
原创 FAST-LIVO2 源码精读（十三）：视觉子系统——稀疏直接法、块对齐与曝光估计
光度残差定义为（recur⋅Icurp−eref⋅Irefpwarprecur⋅Icurp−eref⋅Irefpwarp白话翻译：当前帧亮度乘逆曝光时间减参考帧亮度乘参考逆曝光时间。曝光相同时退化为普通光度差；曝光不同时，两侧均乘各自的e1texpe1/texp，将绝对亮度折算为辐射量，使跨帧比较在物理上一致。完整雅可比链通过中的内循环逐像素计算（
2026-06-24 12:49:49
266
原创 FAST-LIVO2 源码精读（十二）：ESIKF 雷达观测更新源码逐行
本文是「FAST-LIVO2 激光-惯性-视觉里程计源码精读」专栏第十二篇。上一篇精读了 IMU 前向传播与反向去畸变的实现细节；本篇进入 LIO 雷达观测更新的完整闭环，沿→→→调用链，逐层拆解降采样、残差装配、H 矩阵构造、卡尔曼增益求解、迭代收敛判断与地图增量写入的每一个工程细节。
2026-06-24 12:49:24
29
原创 FAST-LIVO2 源码精读（十一）：IMU 处理模块实现——初始化对齐与反向去畸变
本文是「FAST-LIVO2 激光-惯性-视觉里程计源码精读」专栏第十一篇。上一篇解析了多雷达统一预处理与curvature时间字段的编码约定；本篇进入与的实现层，完整拆解四个核心流程：静止初始化与重力对齐、前向传播与 19 维协方差递推、IMUpose 位姿缓存机制，以及双指针逆序反向去畸变的工程细节，并揭示"先存后补"策略的根本动因。
2026-06-24 12:49:03
32
原创 FAST-LIVO2 源码精读（十）：雷达点云预处理与多雷达接口
这是一个刻意的工程妥协：为避免自定义点云类型带来的 PCL/ROS 序列化兼容成本，系统借用了现有字段传递时间信息。
2026-06-23 12:53:05
152
原创 FAST-LIVO2 源码精读（九）：VoxelMap 体素地图——哈希索引与八叉树平面拟合
本文是「FAST-LIVO2 激光-惯性-视觉里程计源码精读」专栏第九篇。上一篇解析了三路传感器同步与状态机的调度逻辑；本篇深入 FAST-LIVO2 地图管理的核心——VoxelMap：剖析哈希体素定位如何实现 O(1) 查询、八叉树如何自适应细分、平面拟合的最小特征值判据与6×6不确定度矩阵如何服务于 ESIKF 残差加权，并揭示其相对前作 ikd-Tree 的设计取舍与工程优势。
2026-06-23 12:52:48
94
原创 FAST-LIVO2 源码精读（八）：系统入口、多传感器同步与 lio_vio_flg 状态机
本文是「FAST-LIVO2 激光-惯性-视觉里程计源码精读」专栏第八篇，也是源码实现篇章（第 8–14 篇）的开篇。前七篇完成了系统认知（第 1–3 篇）与数学地基（第 4–7 篇）；从本篇起进入各模块的源码精读。本篇聚焦系统入口LIVMapper的同步协调机制：三路传感器缓冲队列的工程设计、如何按时序组装测量包，以及状态机如何在 LIO 与 VIO 两种更新模式之间精确切换。
2026-06-18 12:47:03
389
原创 FAST-LIVO2 源码精读（六）：ESIKF 推导与迭代收敛
FAST-LIVO2 的完整状态x\mathbf{x}x旋转矩阵 位置 逆曝光时间 速度 陀螺零偏 加计零偏 重力向量由于R∈SO3R∈SO3是三维流形，误差状态δx∈R19δx∈R19δxx⊖xLogR⊤Rp−pe−1−e−1v−vbg−bgba−bag−gδxx⊖xLogR⊤Rp−pe−1−e−1v−vbg−bgb。
2026-06-17 12:44:59
167
原创 FAST-LIVO2 源码精读（五）：IMU 运动模型、离散传播与去畸变原理
本文是「FAST-LIVO2 激光-惯性-视觉里程计源码精读」专栏第五篇，数学基础篇章（第 4–7 篇）的第二篇。上一篇在 SO(3) 流形上建立了姿态表示与右扰动模型；本篇聚焦 IMU 预积分的核心问题：一帧激光扫描横跨约 100 ms，帧内每一个点的采集时刻各不相同，如何利用 200 Hz 的 IMU 量测将全帧点云统一对齐到帧末时刻？答案完整封装在（587 行）的函数中，本篇逐段拆解其四阶段逻辑，并推导协方差矩阵随误差状态转移矩阵F_x逐帧演化的物理含义。
2026-06-17 12:44:38
156
原创 FAST-LIVO2 源码精读（四）：SO(3) 流形、李代数与右扰动
本文是「FAST-LIVO2 激光-惯性-视觉里程计源码精读」专栏第四篇，也是数学基础篇章（第 4–7 篇）的开篇。前三篇完成了系统认知与实际运行；从本篇起深入数学，为后续 ESIKF 推导和源码逐行精读铺垫必要的工具。本篇聚焦一个根本问题：旋转为什么不能直接相加，以及 FAST-LIVO2 的operator+是如何在代码层面解决这个问题的。
2026-06-15 12:53:15
303
原创 FAST-LIVO2 源码精读（三）：数据集跑通与 RViz 可视化
本文是「FAST-LIVO2 激光-惯性-视觉里程计源码精读」专栏第三篇。前两篇完成了系统认知的建立与编译环境的搭建；本篇进入第一次真正的运行：从数据集获取、launch 启动，到 RViz 各显示项的含义，再到 PCD 点云与轨迹的落盘，完整走完一次官方数据的回放流程，并在结尾说明自录数据接入的修改要点。
2026-06-15 12:52:52
302
原创 FAST-LIVO2 源码精读（二）：环境搭建与编译避坑
本文是「FAST-LIVO2 激光-惯性-视觉里程计源码精读」专栏第二篇。上一篇确立了系统全局认知与 19 维状态向量的精确构成；从这一篇起进入实战。编译通过是阅读源码、调参、改代码的前提——在此之前任何"理解"都悬在空中。本篇逐项拆解 FAST-LIVO2 的依赖树，重点讲清三处极易踩错的版本地雷，并给出常见报错的速查处置表。
2026-06-14 09:34:19
101
原创 FAST-LIVO2 源码精读（一）：系统总览——从 LIO 到 LIVO 的多传感器融合架构
本文是「FAST-LIVO2 激光-惯性-视觉里程计源码精读」专栏的开篇。全专栏共 18 篇，锚定香港大学 MaRS 实验室 2025 年 1 月开源的官方代码，逐模块剖析这套登上 T-RO’24 的多传感器融合系统。开篇先建立全局认知：FAST-LIVO2 所要解决的问题、整体框架的构成，以及源码主线的数据流动。
2026-06-14 09:33:16
583
1
原创 ORB-SLAM3代码详解 - 第 18 篇 · Atlas 多地图系统、地图保存与全系统调优
会跑：理解三线程架构与数据流（01/08/10/13），能编译、配参、复现各数据集结果。会改：掌握相机模型抽象（05）、g2o 顶点与边（14）、预积分（15），能接入新传感器、新相机、新约束。会调：理解每个参数背后的物理含义与耦合（03/15/16/18），能针对自己的平台与场景做精度/实时/鲁棒的权衡。ORB-SLAM3 的价值不止于一个可用的 SLAM 系统，更在于它是一套工程化的几何状态估计模板：清晰的模块边界、可替换的相机/传感器抽象、紧耦合优化的标准范式。
2026-06-13 08:44:20
171
原创 ORB-SLAM3代码详解 - 第 17 篇 · 视觉惯性紧耦合优化
（约 492 行）、的EdgeGyroRWEdgeAccRW（约 2383 行）、（约 4491/4875 行），（约 1738 行）。第 16 篇完成 VI 初始化、给出尺度/重力/零偏的良好初值后，系统进入稳态：每帧跟踪与每个局部滑窗都把视觉重投影约束与 IMU 预积分约束放进同一个优化问题。本篇分析紧耦合的因子图结构、惯性边的解析雅可比、帧级与窗口级两层优化，以及边缘化先验。所有行号、函数签名、常量值均基于仓库 commit4452a3c；
2026-06-13 08:43:46
45
原创 ORB-SLAM3代码详解 - 第 16 篇 · 视觉惯性初始化：尺度、重力与零偏估计
视觉与惯性各有盲区，单独都无法给出完整、有度量的状态：二者互补，但耦合在一起：要用 IMU 给视觉定尺度，必须先知道零偏和重力；要估零偏和重力，又得有可靠的相对运动参考。需要联合估计五组量：{ s (尺度),Rwg (重力方向),bg (陀螺零偏),ba (加速度计零偏),vi (各关键帧速度) }\big\{\;s\ (\text{尺度}),\quad \mathbf{R}_{wg}\ (\text{重力方向}),\quad \mathbf{b}_g\ (\text{陀螺零偏}),\quad \ma
2026-06-12 12:46:16
181
空空如也
空空如也
TA创建的收藏夹 TA关注的收藏夹
TA关注的人