FAST-LIO源码解析与工程实践：LiDAR+IMU紧耦合SLAM避坑指南

简介：针对FAST-LIO论文与源码的个人理解笔记，适合正在学习激光雷达与惯性组合导航、希望从数学推导落到代码实现的SLAM初学者和研究者。笔记从李群李代数与流形基础开始，先解释旋转矩阵与位姿变换为何采用流形表达，再梳理点云坐标格式、Velodyne VLP-16传感器扫描特性、ROS话题通信机制以及C++实现中的数据结构与内存管理要点；随后进入FAST-LIO核心框架，详细说明LiDAR点云预处理、IMU初始化、状态估计中的迭代误差状态卡尔曼滤波流程，并解释ikd-tree如何维护增量式地图、通过区域删改与平衡操作实现高效邻域搜索。后半部分聚焦源码，依次分析主程序、IMU数据处理、激光建图与ESKF更新等关键模块，附带作者在理解和调试过程中遇到的疑问、问题清单与参考文献，便于对照开源仓库逐行阅读。资源共1个文件，为docx格式，压缩包大小5.09MB，目录层级分明，篇章结构贴近论文组织方式。目前已有3113人学习下载，适合需要系统拆解FAST-LIO原理与代码实现的读者深入学习。

1. FAST-LIO 到底在做什么：LiDAR+IMU 紧耦合的直觉与边界

FAST-LIO 是近几年 LiDAR+IMU 紧耦合 SLAM 方案里被复现最多的一种，这份阅读笔记帮我省掉了大量对公式与代码对应关系的时间。它的核心做法是不提特征，直接用去畸变后的点云去构建点面残差，再通过迭代卡尔曼滤波把 IMU 预测和激光观测融在一起。适合谁看？已经跑通过 LOAM 系里程计、想进一步理解状态估计和树结构的人，以及准备换传感器平台、需要知道哪些参数和哪些代码耦合的人。它的边界也很明确：外参标定质量和时间同步精度是前置条件，这两个没做好的话，后面调什么都白搭。

2. 数学地基：李群流形与 ikd-tree，理解状态更新的两个关键

2.1 李群与李代数：为什么旋转增量要用 Exp / Log 而不是直接加减

FAST-LIO 的状态估计里，旋转是用旋转矩阵表示的，但更新时不会拿旋转矩阵做加法。笔记 1.1 节把这条链路写得很完整：旋转向量 r = θa，表示绕单位轴 a 旋转 θ 角；旋转矩阵 R 有行列式约束和正交性约束，直接对它的元素做加法，结果大概率不是合法旋转。所以系统引入了指数映射 exp(r∧) 和对数映射 Log(R)，把旋转增量放在李代数上计算，再用指数映射收回流形。

这套思路落到代码上，常见做法是借助 Sophus 库来管理李群运算，而不是自己写矩阵指数：

// 按 SO(3) 流形方式给旋转加一个微小扰动 state.rot = state.rot * Sophus::SO3d::exp(angular_velocity * dt);

这段代码的逻辑是：当前旋转矩阵乘以角速度组成的李代数小量经指数映射后得到的结果，就是前向传播后的旋转。如果在代码里偷懒写成矩阵加法，数值误差会逐渐积累，最后旋转矩阵不再满足正交约束，整个状态估计发散。参数上这里最需要注意的是 dt 的取值——IMU 频率 200Hz 时 dt 是 0.005 秒，如果错误地用激光雷达帧间隔 0.1 秒去积分角速度，旋转增量会被放大 20 倍，第一帧就会飞出地图。

笔记附录 A 提到旋转矩阵求导“看不懂，暂时放弃”，这个心态很真实。实际工程里可以从李代数小量出发，借助链式法则绕开直接对矩阵求导，结果完全一致。卡在推导上的话，不妨先把代码跑通再回头补数学，效率反而高。

2.2 流形运算：⊞ 和 ⊟ 是状态增量的定义方式

流形这个概念如果不落地到具体运算，很容易变成玄学。笔记 1.2 节把流形运算抽象得很简短：对 SO(3) 来说，R ⊞ r = R * Exp(r)，R1 ⊟ R2 = Log(R2ᵀ R1)；对普通的欧氏空间，⊞ 就是加法，⊟ 就是减法。

这套运算在 esekf 里是状态更新的统一接口。预测阶段得到的是李代数小量，必须通过 ⊞ 注入状态；两个状态之间做差值求残差，也必须用 ⊟ 而不是直接相减。如果自己扩展 FAST-LIO 去加新状态量，比如在线估计加速度计零偏，就必须在状态结构体里同时定义对应的 ⊞⊟ 规则，否则迭代卡尔曼的增益更新会得到非法状态。

还有一个容易错的细节：流形维度不等于状态向量长度。SO(3) 是三维流形，但旋转矩阵在内存里有 9 个元素，所以协方差矩阵的尺寸按流形维度计算，不是按矩阵元素个数计算。esekf 里状态向量展开成列向量时，旋转部分只占 3 维，九个数是展平后的容身之处，如果搞混了这个，后面所有雅可比矩阵的维度都会错位。笔记附录 B 里对 STATE_IKFOM 的展开就是对这个问题最好的注释——它把每个状态变量在向量里的位置标得清清楚楚，读代码前先把这个展开看一遍，能少走很多弯路。

2.3 ikd-tree：为什么要增量维护而不是每帧重建

ikd-tree 是 FAST-LIO 相对早期 LOAM 系方案的关键差异。LOAM 系通常维护一个全局点云地图，定期重建 kd-tree；FAST-LIO 只在局部地图里做配准，而局部地图的点在持续增删，如果用标准 kd-tree 每帧重建，单帧耗时会随地图规模线性上涨。ikd-tree 把增量插入和删除直接作用在树结构上，只有树失衡到一定程度才做重建，这样平摊下来代价小很多。

ikd-tree 的日常使用里几个参数影响很大：

• 树的最大点容量：超出后会触发 rebalance，容量设得太小重建频繁，设得太大内存占用高；
• 删除是“懒惰”的：删除标记只做记录，延迟到重建时才物理清理，这在窗口滑动的局部地图场景下能省大量重排开销；
• 失衡阈值：决定子树何时重建，默认值在室内外一般场景表现稳定，但在长走廊这种退化环境下可能需要放松阈值来减少重建次数。

实际调试时最常见的现象是跑着跑着 CPU 占用越来越高，排除了配准问题之后，十有八九是局部地图半径设得太大，ikd-tree 里积累了大量远离当前位姿的点，删除标记越堆越多，查询路径变长。这时候不是怀疑算法有问题，而是该缩小局部地图裁剪半径了。

2.4 状态协方差传播：线性化是估计质量的瓶颈

协方差传播在笔记里着墨不多，但源码里 esekf 的前向预测大部分时间都花在协方差更新上。IMU 的角速度和加速度通过状态转移矩阵影响下一时刻的协方差，这个过程需要对状态转移矩阵做线性化。如果在这里为了省时间把雅可比设为单位阵，短期看不出问题，但一旦运动剧烈，协方差会被严重低估，后续迭代卡尔曼的增益算出来偏大，轨迹会出现震荡。

另一种常见误用是把过程噪声设得过大，这会让滤波器更“相信”观测，单帧修正量很大，ESDF 落地点波动明显。过程噪声的本质是描述 IMU 零偏和测量噪声的不确定性，它不是玄学，是需要对着传感器数据手册去估的量。笔记里虽然没有给出一组通用参数，但把噪声进入状态更新的路径解释清楚了，自己调参时就能知道改的是哪个矩阵。

3. 数据流全链路：从 IMU 初始化到点云去畸变的可复现顺序

3.1 数据格式与时间同步：PointCloud2、Imu、Odometry 字段里容易被忽略的边界

FAST-LIO 的数据输入基于 ROS 标准消息。激光雷达一般是sensor_msgs/PointCloud2，IMU 是sensor_msgs/Imu，里程计输出是nav_msgs/Odometry，需要轨迹给下游的话还会有nav_msgs/Path。笔记 1.3 节把每个消息的关键字段都列了出来，这里只讲工程上最容易踩的三个点。

PointCloud2 里的point_step和row_step决定了点云解析时的字节偏移。很多自采点云里除了 x/y/z 还有 intensity、ring、time 字段，读取时必须按 PointField 里的 offset 去取坐标，不能假设 x 一定在字节 0。FAST-LIO 自带预处理如果按 16 线雷达格式写死字段位置，换一个雷达就很可能读到错误坐标，地图出现奇怪的条状断层。

Imu 消息里的协方差字段angular_velocity_covariance和linear_acceleration_covariance经常被填成 0。FAST-LIO 初始化阶段用的是角速度和加速度的方差来做静止检测，协方差全零会导致初始化判据失效。常见做法是先把协方差填成一个小的对角阵，量级参考传感器手册里的噪声密度，等跑起来之后再回头看滤波输出是否合理。

订阅侧的队列长度也值得单独说：

ros::Subscriber sub_imu = nh.subscribe("/imu/data", 200, &ImuProcess::imu_cbk, &imu_process); ros::Subscriber sub_lidar = nh.subscribe("/points_raw", 5, &LaserMapping::lidar_cbk, &laser_mapping);

IMU 消息频率通常是 100-400Hz，队列给 200 是为了防止回调抖动时丢消息；激光点云帧率低很多，队列给 5 就够。IMU 消息一旦丢帧，前向传播的时间戳会断层，严重时直接掉估计。如果跑起来状态跳变而且日志里没有报错，优先查这两个队列长度和话题实际发布频率是否匹配。

3.2 IMU 初始化：不是玄学，是静止判据加协方差设定

FAST-LIO 把 IMU 初始化列为独立步骤，但实际代码里它往往只是“等足够多 IMU 帧”的过程。初始化要解决两个问题：IMU 零偏的粗估计，以及重力方向的初始对齐。工程上常用的判据是窗口内加速度模值的方差低于某个阈值，认为传感器处于静止状态，此时加速度计均值可以分离出重力方向。

这部分算法含量不高，但坑很隐蔽：如果初始化时设备在运动，重力方向估计会带偏差，后续即使 esekf 能收敛，轨迹也会有整体倾斜。我一般会要求录数据时前几秒保持静止，再做快速旋转来充分激励加速度计。静止窗口通常取 1-2 秒，IMU 频率 200Hz 就是 200-400 帧，足够算均值和方差。

初始化完成后才会启动前向传播和雷达配准，所以去畸变流程是依赖初始化结果的。实际调试时不要把初始化当成一个可以跳过的步骤，它直接决定了地图坐标系是否水平。

3.3 点云预处理与畸变矫正：间隔采样与时间对齐的前后顺序

笔记 2.0 节明确说“默认不提取特征，对原始数据间隔式（间隔 3 个点）降采样提取”。这意味着 FAST-LIO 不依赖角点、平面点分类，而是直接把稀疏化后的点云拿去构建残差。这样做的优势是省掉了特征提取的调参，代价是点云密度不足时平面拟合会退化。

可以做一个粗略的预算：16 线雷达一圈大约 3 万点，间隔 3 个点采样后剩 7-8 千点，构建局部地图和查询近邻的开销都在可接受范围。如果换 64 线或固态雷达，间隔参数要重新标定，点云太密不是好事，既增加树维护负担又延长单帧配准时间。

去畸变的本质是把一帧点云内每个点的时间戳还原出来，用 IMU 前向传播得到的位姿把点补偿到统一时刻。常见做法是给每个点赋一个相对帧起始时刻的偏移，然后按时间线性插值取 IMU 姿态。这里的坑是时间字段单位：有的是秒，有的是纳秒，有的是帧内索引，不统一会导致畸变矫正方向错误，地图出现重影。

时间对齐和去畸变的顺序可以按这个流程理解：

# 流程顺序（FAST-LIO 数据处理的实际路径） 1. 接收一帧点云，记录帧起始时间 t_start 和帧结束时间 t_end 2. 查询 IMU 队列，取出 [t_start - margin, t_end + margin] 内的 IMU 帧 3. 用 IMU 前向传播得到帧内每个采样时刻的预测位姿 4. 对点云中每个点按时间戳插值位姿，全部变换到同一坐标系 5. 再执行间隔采样降采样，把点云输入给配准模块

第 2 步多取 margin 一般设 10ms 左右，是为了覆盖 IMU 与雷达之间的固定时间延迟。第 4 步的插值方式是线性还是更高级别，取决于 IMU 频率和运动剧烈程度，200Hz IMU 下线性插值通常够用。如果 IMU 频率只有 100Hz 且运动快速旋转，线性插值的残差会变大，这时需要考虑更高阶的插值。

3.4 状态估计流程：把算法图变成可执行的步骤清单

笔记图 8 是状态估计算法流程，图 9 是迭代卡尔曼滤波。这里用表格把步骤和对应源码模块串起来，读代码时按这个顺序找不容易迷路：

理解这张表之后再看源码会顺畅很多。前向传播的结果只是初始猜测，激光观测负责修正。观测构建的质量直接决定修正方向是否正确，而迭代更新里的矩阵运算决定修正幅度。最后的地图更新是给下一帧准备素材，四个步骤环环相扣。

3.5 从数据回放到实时运行：有哪些差异需要提前考虑

很多人会在 rosbag 回放上调试参数，调完直接上实车，结果实车表现远差于回放。这里有个容易被忽略的差异：回放时话题发布节奏均匀，IMU 数据不会丢；实车上可能出现系统调度抖动、串口延迟、CPU 占用波动，导致 IMU 回调间隔不均。FAST-LIO 的前向传播是按时间戳差积分的，间隔不均问题不大，但如果丢帧，问题就严重了。

另外一个差异是点云的时间戳来源。回放时时间戳来自 bag 文件，实车上如果是雷达驱动用收到数据的主机时间打戳，IMU 和雷达之间可能存在几十毫秒的固定延迟。这套延迟如果在回放中不存在或不同，之前调好的参数到实车上就不成立了。所以我的习惯是调试阶段就用带真实时间戳的录制数据，并且有意模拟一定的丢帧抖动，看看系统会不会崩。

4. 源码拆解：主循环、esekf 与激光里程计的真正执行路径

4.1 主程序入口：回调、同步队列和主循环的协作方式

FAST-LIO 的 main 函数做的事不多：初始化 ROS 节点、创建 IMU 处理和激光里程计对象、订阅话题、进入消息循环。真正的逻辑在回调函数和主循环里。激光回调收到点云后并不是立即处理，而是放进缓冲队列等待 IMU 数据对齐；IMU 回调则直接推进前向传播。

主循环最简骨架可以这样理解：

// laserMapping.cpp 主循环简化结构 while (ros::ok()) { ros::spinOnce(); if (lidar_frame_ready && imu_tail_time > lidar_stamp) { // 1. IMU 前向传播，推进状态到激光帧时刻 imu_process->Process(imu_data, lidar_data, state); // 2. 用激光点云残差做迭代更新 kf.update_iterated_dyn_share( input, state, observ_model, solve_observability); // 3. 更新 ikd-tree：插入新点，删除远点 ikdtree.AddPoints(points_body, is_in_map); ikdtree.DeletePoints(points_to_remove); // 4. 发布里程计和 Path publish_odometry(); publish_path(); } }

逻辑说明：Process内部会做 IMU 状态传播、协方差预测和点云去畸变；update_iterated_dyn_share是 esekf 的迭代更新函数，观测模型通过函数指针传入；ikd-tree 的增删在更新完成后执行，为下一帧准备局部地图。参数上需要留意迭代次数的上下界，迭代太少残差收敛不足，迭代太多单帧耗时会涨，一般 2-3 次迭代是性能和精度的折中。

4.2 IMU_Processing.hpp：前向传播里的积分离散化

IMU 处理模块的Process函数职责很集中：把 IMU 测量值按时间顺序对状态做预测。预测公式对应笔记图 7 的前向传播：姿态用角速度积分，速度用加速度积分，位置再积分，零偏作为常数估计，协方差用线性化的状态转移矩阵传播。

实际代码里常见的状态预测片段长这样：

// IMU 前向传播关键片段 state.rot = state.rot * Exp(imu_msg.angular_velocity * dt); state.vel += (state.rot * imu_msg.linear_acceleration - gravity) * dt; state.pos += state.vel * dt;

逻辑说明：第一行完成旋转流形上的积分，第二行把加速度计测量转到世界系后减去重力再积分得到速度增量，第三行把速度积分到位置。参数上最值得留意的是 dt 的计算——IMU 回调之间的时间戳差值如果出现跳动，前向传播会引入额外误差。这也是为什么要求 IMU 时间戳连续且不丢帧。鲁棒性好的代码里通常会加一个保护：如果当前 IMU 时间戳与上一次的差值大于一个阈值，就直接丢弃这次传播，避免跨过大时间缝隙强行积分导致状态飞掉。

4.3 laserMapping.cpp：点面残差怎么构建

FAST-LIO 的配准不提特征点，所以残差构建跟 LOAM 完全不同。拿到一帧去畸变后的点云，先根据当前位姿把点从体坐标系变换到世界坐标系，再到 ikd-tree 里搜索近邻点；如果近邻点足够多，就用它们做平面拟合，残差就是当前点到拟合平面的距离。

残差构建的核心片段可以用伪代码表示：

// 伪代码：构建点面残差 for (auto& p_body : points_body) { Vector3d p_world = state.rot * p_body + state.pos; ikdtree.Nearest_Search(p_world, num_neighbor, neighbor_pts); if (neighbor_pts.size() > min_neighbors) { fit_plane(neighbor_pts, plane_coeff); // 最小二乘拟合平面 residual_dist = fabs((p_world - plane_center).dot(plane_normal)); } }

逻辑说明：Nearest_Search返回局部地图里的近邻点集合，最少邻居数一般取 5，少于 5 个点拟合出的平面法向量不稳定，残差也会失真。fit_plane用最小二乘拟合平面，输出平面法向量和中心点，然后算点到面的距离作为残差。参数上num_neighbor和min_neighbors一个控制查询数量、一个控制拟合下限，如果换用密度差异很大的雷达，这两个参数要跟着采样间隔一起调。还有一层容易忽略的是距离阈值，近邻点如果跨越了两个不同平面，拟合出来的法向量是折中的，这时候要先剔除距离过远的点再拟合。

4.4 esekf 的预测与更新：迭代卡尔曼为什么比 EKF 稳

笔记提到“误差卡尔曼滤波”，对应代码里esekfom::esekf类。esekf 用的是误差状态迭代卡尔曼，它跟普通 EKF 的区别在于：普通 EKF 在预测点附近做一次线性化就结束了，esekf 在更新阶段会做多次线性化，每次用上一次的后验状态重新计算观测雅可比，直到残差收敛或达到迭代上限。

实际收敛判据一般看更新前后状态差值的范数，或者直接固定迭代次数。源码里update_iterated_dyn_share的迭代循环里有大量矩阵求逆和雅可比运算，这也是整个系统最吃 CPU 的地方。想快速判断是不是 esekf 拖了后腿，可以用性能工具看一下这个函数占单帧处理时间的比例，如果超过一半，可以尝试减少迭代上限或简化观测模型。

另一个必须注意的点是状态向量和输入向量的展开顺序，对应笔记附录 B 的STATE_IKFOM和INPUT_IKFOM。状态里包含位姿、速度、IMU 零偏、重力向量，输入只有角速度和加速度。展开时最容易写错的就是位置和各分量的顺序，一旦顺序对不上，观测模型的雅可比维度会全错。很多人调参调不好，最后发现是这里某个索引写错了，属于又隐蔽又耗时的问题。

4.5 代码阅读技巧：typedef、inline 和宏在源码里扮演的角色

读 FAST-LIO 源码时，会看到大量的 typedef 和 inline。笔记 1.7 节对它们的解释很实用：typedef 的核心价值是隐藏复杂类型、简化声明，比如把一大串模板块类型用短别名替代，代码读起来清楚很多；inline 则是把短函数直接在调用处展开，省掉函数调用的开销，这类函数通常只有一两行，比如李代数到矩阵的转换。

还有EIGEN_MAKE_ALIGNED_OPERATOR_NEW这个宏，在很多类的私有部分都有。它是一个对齐内存分配声明，当类里有 Eigen 固定大小矩阵成员时，默认的new分配的内存可能不满足 SIMD 对齐要求，轻则性能下降，重则运行时报错。凡是看到这个宏的地方，都是因为该类持有 Eigen 成员，这是故意为之，不是无关代码。

Eigen::Dynamic这个类型也在源码里出现。它表示矩阵维度在运行时才确定，灵活性高，但性能开销比编译期固定维度大。FAST-LIO 里用动态维度的地方通常是那些帧数可变的矩阵，比如观测矩阵的行数随点云点数变化，这是合理的用法。如果自己改代码时把固定维度全改成动态维度，效率会下降，反过来也不行——固定维度在编译期就确定了，运行时不能塞进不同大小的矩阵。

5. 避坑：参数、时间同步与编译问题里最常翻车的五个点

5.1 时间戳不同步导致建图漂移

现象：跑公开数据集表现正常，换自采数据后轨迹慢慢漂出真实路线，建图重影严重。

原因：IMU 和激光雷达的时间戳不同源，或者两者存在固定延迟。前向传播用的是 IMU 时间戳，点云去畸变用的是点云内时间字段，两套时间一旦对不上，残差就建立在错误的时间对齐上，漂移是必然结果。

解决：录制数据时优先做硬件时间同步；没有硬件同步就做软件对齐。跑数据之前先对比同一段时刻 IMU 和雷达话题的时间戳差值，如果差值波动超过 40ms，就先解决时间同步问题再谈调参。常见做法是让雷达驱动和 IMU 驱动都用同一主机时间打戳，并测量两者的固定延迟在滤波器里补偿。

5.2 Eigen 对齐编译错误

现象：编译能通过，运行时程序直接崩溃，报错信息里能看到unaligned或assertion等关键字。

原因：结构体里有 Eigen 的固定大小矩阵成员，但没有启用对齐内存分配。现代处理器用 SSE/AVX 指令做向量化计算时对内存对齐有要求，不对齐的访问轻则性能下降，重则直接运行时断言失败。

解决：在包含 Eigen 成员的类定义里加EIGEN_MAKE_ALIGNED_OPERATOR_NEW宏，笔记 1.7.1 节解释过它的作用是重载该类的 new 来保证对齐。另一个相关场景是标准容器存 Eigen 类型，比如std::vector<Eigen::Matrix4d>，这时候要显式传入Eigen::aligned_allocator，否则扩容时同样会崩。

5.3 IMU 初始化失败但没报错

现象：启动后激光数据一直在进，但输出里程计长时间不动，或者刚启动就出现一个很大的位姿跳变。

原因：初始化条件没满足。FAST-LIO 的初始化依赖静止状态来分离重力和估计零偏，如果设备在晃动中启动，重力方向和零偏估计都会偏，后续滤波器要花很长时间才能纠正，甚至在快速运动中发散。

解决：录数据时保证启动后 1-2 秒设备静止，启动前先确认 IMU 话题频率稳定。如果自己改过初始化逻辑，注意方差阈值的设定——阈值太严，永远判定不了静止；阈值太松，运动中也触发初始化，两种情况结果都很糟。

5.4 降采样间隔和邻居数不匹配

现象：走廊、长直墙这类场景建图精度还行，一到灌木丛、草地，残差剧烈波动，地图表面噪点变多。

原因：间隔采样后的点云密度和平面拟合的最小邻居数不匹配。点云过于稀疏时，一个点的近邻可能跨越两个不同平面，拟合出来的平面法向量是折中产物，残差失真。

解决：调低采样间隔，同时增大邻居数，或者对近邻点做距离过滤，超出阈值的点先剔除再拟合。实际调参时可以打印每帧有效残差数量，如果数量过少，说明降采样太激进或邻居数阈值太高。这个参数组合是雷达密度相关的，换传感器后一定要重新标定。

5.5 局部地图裁剪参数与场景尺度不匹配

现象：地图能建，但 CPU 占用越来越高，建图帧率一路下跌，时间都耗在树维护上。

原因：局部地图半径设得过大，ikd-tree 里堆积了大量当前位姿附近的旧点，删除标记越积越多，查询路径变长，增量删除的优势被抵消。

解决：缩小局部地图裁剪半径，让树里维持一个稳定规模的点数。场景特征少（比如地下车库）时半径可以放宽，室外开阔环境半径太大收益有限且计算成本高。调完参数后可以观察单帧处理时长，如果稳定在预期范围且不随运行时间增长，说明树维护的压力已经被控制住了。

6. 进阶：验证结果，先看残差曲线再看累计误差

6.1 先看残差曲线：esekf 有没有收敛一目了然

跑完一段数据，除了看轨迹图，我还会把迭代更新阶段的残差模长记录成日志画出来。残差曲线的形态能反映 esekf 是否收敛：如果后两次迭代的残差模长没有明显下降，说明迭代已经收敛，当前参数组合下滤波器的更新幅度有限；如果残差反复震荡，说明观测模型中有雅可比或参数设置不合理，或者近邻点数太少导致平面拟合不稳定。

把残差和协方差的范数记录到同一份日志里还有个额外好处：能看出运动剧烈程度对协方差的影响。快速转弯时协方差会有一个明显的尖峰，这是正常现象；如果直线匀速段协方差也在不断膨胀，就要检查过程噪声是不是设得过大。

6.2 往返测试与离线地图对比：定量判断改动效果

我的第二个验证习惯是固定一段起点与终点重合的数据做往返测试。从起点出发绕一圈回到起点，看里程计输出的终点偏差在什么量级。这比单程轨迹直观得多，因为累计漂移在闭合回路上会被直接暴露出来。同一份数据录制三次对比轨迹一致度，如果三次结果互相对齐度差，优先怀疑时间同步和 IMU 初始化，而不是滤波器参数。

第三个验证是离线对比。把每帧点云和对应位姿保存下来，用日志重建地图，再对比在线建图结果。如果离线重建效果更好，说明滤波更新里有某个环节在实时压力下丢失了解析精度；如果离线结果一样差，问题出在前端的残差构建阶段，跟实时性无关。从那以后，我每次改完参数或者动完源码，都会强制走一遍“先看残差、再跑往返、最后对比离线地图”这个流程。这套流程帮我把排查时间压缩了一大半，也希望帮到你。

本文还有配套的精品资源，点击获取