自动驾驶定位实战：如何用IMU和GPS实现厘米级精度（附ROS代码解析）

在自动驾驶的感知世界里，定位是车辆理解“我在哪里”的基石。没有精准的定位，再聪明的决策系统也无从谈起。对于许多工程师和研究者而言，理论上的多传感器融合方案听起来很美，但一旦进入ROS环境，面对纷繁的数据流、时间戳对齐和状态估计代码，挑战才真正开始。这篇文章不打算复述教科书上的卡尔曼滤波公式，而是直接切入工程现场，分享如何将IMU的高频动态响应与GPS的绝对位置锚定结合起来，在真实的代码层面实现稳定、可靠的厘米级定位。我们会从数据预处理开始，一步步拆解IMU预积分的实现细节、GPS数据与局部坐标系的转换技巧，并最终构建一个能够在城市峡谷和短暂隧道中保持鲁棒性的融合定位节点。如果你正在为定位模块的飘移、跳变或融合效果不佳而头疼，希望这里的实战经验能给你带来一些新的思路。

1. 理解黄金搭档：IMU与GPS的互补哲学

在传感器选型时，我们常常追求“全能”的器件，但现实是，每种传感器都有其物理极限。IMU和GPS之所以被称为定位领域的“黄金搭档”，恰恰是因为它们在几乎所有关键特性上都形成了完美的互补，这种互补性不是简单的叠加，而是深层次的相互校正与增强。

首先从数据频率来看，商用级IMU的输出频率轻松达到100Hz甚至更高，这意味着它能够捕捉车辆每一个微小的加速度变化和转向动作。相比之下，常见的车载GPS接收机频率通常在1Hz到10Hz之间。如果单独依赖GPS，我们得到的车辆轨迹就像是一系列稀疏的点，点与点之间的运动状态完全是猜测。而IMU的高频特性完美地填补了这些空白，提供了连续、平滑的运动估计。

更核心的互补在于误差特性。IMU通过积分计算位姿，其误差会随时间累积，这就是所谓的“漂移”。你可能在实验室里静止不动，但IMU解算出的位置却会慢慢“走”出去。GPS则完全不同，它的每一次测量在理论上都是独立的，只受当前卫星信号质量的影响，不存在累积误差。但是，GPS信号非常脆弱，高楼、树木、隧道，甚至天气都可能让它瞬间失准或完全丢失。一个会随时间漂移但不受环境干扰，一个绝对准确但环境适应性差，将它们结合起来，正好用GPS的绝对位置来周期性地校正IMU的漂移，同时用IMU的连续性来弥补GPS的信号中断。

从数据本质上看，IMU提供的是相对运动信息。它告诉你：“在过去的0.01秒内，我向前加速了0.1米/秒²，并向右旋转了0.5度。”它不知道自己在地球上的绝对位置。GPS提供的是绝对位置信息（在WGS-84坐标系下的经纬高）。融合的核心，就是建立一个状态估计模型，用IMU的相对运动来预测状态，用GPS的绝对位置来更新和修正这个预测。

提示：在实际工程中，不要将GPS数据直接当作“真值”。即使是RTK-GNSS，在复杂环境下也可能出现厘米级的跳变。一个稳健的融合算法必须能检测并处理这种异常。

为了更直观地对比这对搭档，我们可以看下面的特性对照表：

特性维度	IMU (惯性测量单元)	GPS (全球定位系统)	融合价值
数据频率	高 (100Hz+)	低 (1-10Hz)	IMU填补GPS采样间隔内的运动细节
输出类型	相对位移、姿态变化	绝对位置 (经纬高)	将相对运动与绝对锚点结合
误差特性	随时间累积漂移	无累积误差，但存在瞬时噪声和多径效应	GPS校正IMU漂移，IMU平滑GPS噪声
环境鲁棒性	极强，不受外部环境影响	弱，受建筑物、天气、遮挡影响大	IMU在GPS失效时提供短期航位推算
初始化需求	需要初始姿态（可从加速度计静态估算）	需要时间搜索卫星	结合两者可快速获得初始位姿

理解了这种哲学层面的互补，我们才能在设计融合框架时做出正确的权衡。例如，在GPS信号良好的开阔地带，我们应该更信任GPS的观测；而在进入隧道的一刹那，系统应能自动降低GPS的权重，更多地依赖IMU的短期推算，并为GPS的回归设计一个平滑的融合过渡逻辑。

2. 工程基石：ROS下的传感器数据同步与预处理

在开始写融合算法之前，一个更基础、却往往更耗时的环节是处理好数据管道。ROS提供了强大的通信机制，但不同传感器数据流异步到达、时间戳微妙差异、坐标系不统一等问题，足以让一个理论上完美的算法在实践中崩溃。我们的目标是构建一个“干净”的输入接口。

时间同步是第一个拦路虎。IMU数据包自带的时间戳（header.stamp）和GPS数据的时间戳可能来自不同的硬件时钟。直接使用ROS的message_filters库中的ApproximateTime策略是一个不错的起点。它可以近似同步不同话题上的消息。例如，我们需要同步/imu/data和/gps/fix话题：

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

message_filters::Subscriber<sensor_msgs::Imu> imu_sub(nh, "/imu/data", 100);
message_filters::Subscriber<sensor_msgs::NavSatFix> gps_sub(nh, "/gps/fix", 100);

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Imu, sensor_msgs::NavSatFix> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), imu_sub, gps_sub);
sync.registerCallback(boost::bind(&callback, _1, _2));

但要注意，ApproximateTime策略要求消息频率不能相差太大。如果IMU是100Hz，GPS是10Hz，它通常会等待时间上最接近的一对消息进行回调。更精细的做法是维护一个IMU数据的滑动窗口缓冲区，当GPS消息到达时，从缓冲区中选取时间戳最接近的IMU数据，甚至进行插值，来获得更精确的同步状态。

坐标转换是第二个关键步骤。GPS给出的位置通常是WGS-84大地坐标（经纬度、高度），而我们的车辆定位和地图通常使用局部笛卡尔坐标系（如UTM坐标系下的东-北-天）。我们需要一个可靠且高效的转换。在ROS中，robot_localization包或GeographicLib库是完成这项工作的利器。下面是一个将GPS数据转换到局部UTM坐标的示例片段：

#include <geographic_msgs/GeoPoint.h>
#include <robot_localization/navsat_conversions.h>

double northing, easting;
std::string utm_zone;
// 假设 origin_lla 是预先设定的局部坐标系原点（经纬高）
robot_localization::NavSatConversions::LLtoUTM(
    gps_msg->latitude, gps_msg->longitude,
    northing, easting, utm_zone);

// 计算相对于原点的坐标
double local_x = easting - origin_easting;
double local_y = northing - origin_northing;
double local_z = gps_msg->altitude - origin_altitude;

这里有一个工程细节：原点的选择很重要。通常选择第一次收到稳定GPS信号的位置作为局部坐标系原点，以避免大数字运算带来的精度损失。

数据有效性检查与滤波是预处理中常被忽视但至关重要的环节。对于IMU，在静态初始化时，我们可以通过计算加速度计数据的方差来判定车辆是否真正静止，以准确估算初始的滚转和俯仰角。对于GPS，需要检查status.status字段，并关注position_covariance（位置协方差）矩阵。一个陡然增大的协方差往往意味着GPS信号质量下降（如进入高架桥下），融合算法此时应该降低该观测值的权重。

• IMU预处理清单：
  • 零偏校正：在系统启动静止时，采集数秒数据计算加速度计和陀螺仪的零偏，并在后续数据中扣除。
  • 坐标系对齐：确认IMU的机体坐标系（x, y, z）与车辆坐标系（前-左-上）的对应关系，必要时进行旋转。
  • 重力补偿：在将加速度转换到世界坐标系时，需要减去重力矢量。
• GPS预处理清单：
  • 状态检查：只处理status.status >= sensor_msgs::NavSatStatus::STATUS_FIX的数据。
  • 精度过滤：根据position_covariance[0]和position_covariance[4]（东、北方向方差）设置阈值，过滤掉精度过低的数据。
  • 跳变检测：计算连续两次GPS位置的距离差，如果超过一个基于车速的动态阈值，则视为异常跳变并舍弃。

把数据同步、坐标转换和有效性检查这些“脏活累活”做扎实，后续的融合算法才能在一个可靠的基础上运行，否则再高级的滤波算法也如同在流沙上盖楼。

3. 核心算法拆解：IMU预积分与状态预测

当我们谈论融合IMU数据时，最直接的想法是在世界坐标系下对加速度进行双重积分得到位置。但这种方法在优化框架中会遇到一个严重问题：每次我们优化调整了k时刻的位姿，k时刻之后所有依赖于该位姿的IMU积分都需要推倒重来，计算量巨大。IMU预积分技术就是为了解决这个耦合问题而生的。它的核心思想是：不直接积分得到绝对位姿，而是计算相邻两个关键帧（例如，两个GPS观测时刻）之间的相对运动增量。

假设我们在i时刻和j时刻（j > i）有视觉或激光雷达的关键帧，中间有若干IMU测量。预积分的目标是计算出从i到j的旋转、速度和位置变化量，而这些变化量只依赖于IMU测量值和i时刻的零偏，与i时刻的世界位姿无关。这样，当后端优化调整了i时刻的位姿时，这个预积分量不需要重新计算，只需根据零偏的变化进行一阶近似修正，效率极高。

让我们在ROS节点中实现一个简化的预积分器。首先，定义预积分状态量：

class IMUPreintegrator {
public:
    Eigen::Vector3d delta_p; // 位置增量 (预积分)
    Eigen::Vector3d delta_v; // 速度增量
    Eigen::Quaterniond delta_q; // 旋转增量（四元数）
    Eigen::Vector3d linearized_ba; // 加速度计零偏（线性化点）
    Eigen::Vector3d linearized_bg; // 陀螺仪零偏（线性化点）
    double delta_time; // 积分时间段

    // 协方差矩阵和雅可比矩阵，用于传播噪声
    Eigen::Matrix<double, 15, 15> covariance;
    Eigen::Matrix<double, 15, 15> jacobian;

    IMUPreintegrator() {
        reset();
    }

    void reset() {
        delta_p.setZero();
        delta_v.setZero();
        delta_q.setIdentity();
        covariance.setZero();
        jacobian.setIdentity();
        delta_time = 0;
        // 初始化零偏，可以从外部传入或设为0
        linearized_ba.setZero();
        linearized_bg.setZero();
    }
};

预积分的递推过程在中值积分方法下进行，这种方法比欧拉积分更精确。每当收到一个新的IMU数据，我们进行如下更新：

void integrate(const sensor_msgs::Imu::ConstPtr& imu_msg) {
    double dt = imu_msg->header.stamp.toSec() - current_time;
    if (dt <= 0) return;

    // 1. 提取角速度和加速度，并扣除当前估计的零偏
    Eigen::Vector3d gyr(imu_msg->angular_velocity.x,
                        imu_msg->angular_velocity.y,
                        imu_msg->angular_velocity.z);
    gyr -= linearized_bg;

    Eigen::Vector3d acc(imu_msg->linear_acceleration.x,
                        imu_msg->linear_acceleration.y,
                        imu_msg->linear_acceleration.z);
    acc -= linearized_ba;

    // 2. 中值积分
    Eigen::Vector3d un_gyr = 0.5 * (prev_gyr + gyr); // 上一时刻和当前时刻角速度的平均
    Eigen::Quaterniond dq(1, 
                          un_gyr.x() * dt / 2,
                          un_gyr.y() * dt / 2,
                          un_gyr.z() * dt / 2);
    dq.normalize();

    // 3. 更新旋转、速度、位置增量
    // 旋转：四元数乘法
    delta_q = delta_q * dq;
    // 速度：注意将加速度旋转到上一时刻的机体坐标系下
    Eigen::Vector3d un_acc = 0.5 * (prev_delta_q * acc + delta_q * acc);
    delta_v += un_acc * dt;
    // 位置：使用平均速度
    delta_p += delta_v * dt + 0.5 * un_acc * dt * dt;

    // 4. 更新噪声协方差和雅可比矩阵（此处省略详细推导和代码，涉及离散时间下的误差状态传递）
    // covariance = F * covariance * F.transpose() + V * noise_cov * V.transpose();
    // jacobian = F * jacobian;

    prev_gyr = gyr;
    prev_delta_q = delta_q;
    delta_time += dt;
    current_time = imu_msg->header.stamp.toSec();
}

这个预积分器在两个GPS观测时刻之间运行。当新的GPS数据到来时，我们就得到了一个连接这两个时刻的“约束”：预积分计算出的相对运动，应该等于从GPS观测推导出的相对运动（考虑零偏误差）。这个约束就可以作为图优化（如g2o、GTSAM）中的一个边，或者作为扩展卡尔曼滤波（EKF）中的一个观测模型。

注意：上面的代码是一个高度简化的示意，用于说明原理。完整的实现必须包含对零偏的雅可比矩阵计算、噪声协方差的传播，以及当零偏估计更新后，对预积分量的一阶近似修正（即bias correction）。忽略这些，精度会大打折扣。

通过预积分，我们将高频IMU数据压缩成了关键帧之间的简洁约束，极大地降低了后端优化的计算负担，同时保持了IMU测量的全部信息。这是现代视觉惯性里程计（VIO）和激光惯性里程计（LIO）的核心，同样也适用于我们IMU+GPS的融合定位系统。

4. 构建融合滤波器：从EKF到图优化

有了高质量的传感器数据和IMU预积分约束，下一步就是选择一个状态估计框架来融合它们。工程上最常用的两种方法是扩展卡尔曼滤波（EKF）和基于图优化的方法。两者没有绝对的优劣，选择取决于你对系统实时性、精度以及代码复杂度的权衡。

扩展卡尔曼滤波（EKF） 是一种递归的、轻量级的方案。它维护一个当前时刻的状态向量（通常是位置、速度、姿态以及IMU零偏）和协方差矩阵。其过程分为两步：

1. 预测步：利用IMU数据（或预积分结果）向前推演（预测）下一个时刻的状态和不确定性。
2. 更新步：当GPS观测到来时，将预测的状态转换到观测空间（即计算预期的GPS读数），与真实的GPS观测进行比较，产生残差，然后用卡尔曼增益来修正状态估计。

在ROS中，你可以自己实现EKF，也可以使用robot_localization包中的ekf_localization_node。自行实现可以更灵活地定制运动模型和观测模型。状态向量通常设计为16维： [p_x, p_y, p_z, v_x, v_y, v_z, q_w, q_x, q_y, q_z, b_a_x, b_a_y, b_a_z, b_g_x, b_g_y, b_g_z] 其中p是位置，v是速度，q是姿态四元数，b_a和b_g是加速度计和陀螺仪的零偏。

EKF的优点是计算量小，运行效率高，非常适合对实时性要求极高的控制回路。但其线性化假设在车辆进行急转弯等剧烈运动时可能引入误差，且它本质上是“一阶马尔可夫”的，只考虑当前状态，无法像优化那样利用历史信息进行全局调整。

基于图优化的方法则提供了更高的精度和更好的全局一致性。它将定位问题建模为一个图（Graph）：

• 节点：需要估计的状态（各个时间点的位姿、速度、零偏等）。
• 边：连接节点的约束。有两种边：
  • IMU预积分边：连接相邻时间节点，约束来自于两个节点之间的IMU测量。
  • GPS观测边：连接某个时间节点，约束来自于该时刻的GPS绝对位置测量。

优化器的目标就是调整所有节点的状态，使得这些边所代表的约束（预测值与观测值之差）的总体误差最小。常用的优化库有g2o、GTSAM和Ceres Solver。下面是一个使用GTSAM定义因子图的大致框架：

#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

// 1. 创建因子图和初始估计
gtsam::NonlinearFactorGraph graph;
gtsam::Values initialEstimate;

// 2. 添加先验因子（对第一个状态的粗略估计）
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = ...
graph.addPrior(poseKey1, priorPose, priorNoise);

// 3. 循环处理数据
for (每个时间间隔) {
    // 3.1 进行IMU预积分，得到预积分测量和噪声模型
    PreintegratedCombinedMeasurements preintegrated = imuPreintegrator.preintegrate(...);
    
    // 3.2 添加IMU因子（连接当前状态和下一个状态）
    gtsam::CombinedImuFactor imuFactor(poseKey_i, velKey_i,
                                       poseKey_j, velKey_j,
                                       biasKey_i, preintegrated);
    graph.add(imuFactor);
    
    // 3.3 如果此时有GPS数据，添加GPS因子
    if (has_gps_measurement) {
        gtsam::GPSFactor gpsFactor(poseKey_j, gps_position, gps_noise_model);
        graph.add(gpsFactor);
    }
    
    // 3.4 为新的状态变量设置初始估计值（可以用IMU递推得到）
    initialEstimate.insert(poseKey_j, predictedPose);
    initialEstimate.insert(velKey_j, predictedVel);
}

// 4. 优化
gtsam::LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
gtsam::Values result = optimizer.optimize();

图优化能给出更优的估计，因为它同时考虑了所有历史观测。但其计算量随节点数增加而增长，通常不能像EKF那样逐帧输出，而是以“关键帧”为单位进行局部或全局优化，输出略有延迟。在实际系统中，常采用“滑动窗口”优化，只保留最近一定数量的节点在图中，以平衡精度和效率。

方法	扩展卡尔曼滤波 (EKF)	基于图优化的方法
核心思想	递归贝叶斯估计，在线滤波	批量最小二乘优化，调整所有状态
计算效率	高，O(n)复杂度，常数时间更新	较低，随节点数增加，需迭代求解
精度	中等，受线性化误差影响	高，能减少累积误差，全局一致
内存与历史	只保留当前状态，无历史信息	保留窗口内所有状态，可利用历史
实现复杂度	相对简单	较复杂，需定义因子图和优化器
输出延迟	几乎无延迟	有一定延迟（等待优化完成）
适用场景	对实时性要求极高的控制回路	对精度要求高，可接受轻微延迟的定位建图

在自动驾驶定位模块中，一个常见的混合架构是：前端使用一个轻量级的EKF或互补滤波器进行高频（100Hz）的状态预测与融合，为控制模块提供实时位姿；后端运行一个低频（例如10Hz）的滑动窗口图优化，对前端的状态进行精细校正，并估计IMU零偏等参数，再将优化后的零偏反馈给前端滤波器。这种架构兼顾了实时性与精度。

5. ROS实战：代码解析与调试技巧

理论最终要落地为代码。让我们聚焦于一个具体的ROS节点实现框架，并探讨几个关键的调试技巧。假设我们的节点名为imu_gps_fusion_node，它订阅/imu和/gps话题，发布融合后的位姿/fused_pose。

节点核心结构：

class ImuGpsFusionNode {
public:
    ImuGpsFusionNode(ros::NodeHandle& nh, ros::NodeHandle& pnh) {
        // 1. 参数读取
        pnh.param("use_ekf", use_ekf_, true);
        pnh.param("utm_origin_lat", origin_lat_, 0.0);
        // ... 读取其他参数，如噪声协方差

        // 2. 初始化转换工具和滤波器
        initUtmConverter();
        if (use_ekf_) {
            initEKF();
        } else {
            initGraphOptimizer();
        }

        // 3. 订阅与发布
        imu_sub_ = nh.subscribe("/imu", 100, &ImuGpsFusionNode::imuCallback, this);
        gps_sub_ = nh.subscribe("/gps", 10, &ImuGpsFusionNode::gpsCallback, this);
        pose_pub_ = nh.advertise<geometry_msgs::PoseStamped>("/fused_pose", 10);
        path_pub_ = nh.advertise<nav_msgs::Path>("/fused_path", 10);

        // 4. 初始化状态标志
        is_initialized_ = false;
        last_imu_time_ = 0;
    }

private:
    void imuCallback(const sensor_msgs::Imu::ConstPtr& msg);
    void gpsCallback(const sensor_msgs::NavSatFix::ConstPtr& msg);
    void publishFusedPose(const ros::Time& stamp, const Eigen::Vector3d& pos, const Eigen::Quaterniond& quat);

    // 滤波器实例
    std::unique_ptr<ExtendedKalmanFilter> ekf_;
    std::unique_ptr<GraphOptimizer> graph_opt_;
    // 预积分器
    IMUPreintegrator preintegrator_;
    // 其他工具和状态变量...
};

在imuCallback中，我们不仅进行预积分，还执行一个高频的预测步（如果使用EKF）：

void imuCallback(const sensor_msgs::Imu::ConstPtr& msg) {
    // 1. 数据检查和预处理（去零偏，坐标系转换）
    processImuData(msg);

    if (!is_initialized_) {
        // 尝试初始化：等待有效的GPS数据来获取初始位置，并用静止IMU数据估算初始姿态和零偏
        return;
    }

    // 2. 更新预积分器
    preintegrator_.integrate(msg);

    // 3. 如果使用EKF，执行预测步
    if (use_ekf_) {
        double dt = msg->header.stamp.toSec() - last_imu_time_;
        if (dt > 0) {
            ekf_->predict(dt, processed_acc_, processed_gyr_);
            // 发布预测的位姿
            publishFusedPose(msg->header.stamp, ekf_->getPosition(), ekf_->getOrientation());
        }
    }
    last_imu_time_ = msg->header.stamp.toSec();
}

在gpsCallback中，我们执行融合的核心——更新步：

void gpsCallback(const sensor_msgs::NavSatFix::ConstPtr& msg) {
    if (msg->status.status < sensor_msgs::NavSatStatus::STATUS_FIX) {
        ROS_WARN_THROTTLE(1, "GPS fix invalid.");
        return;
    }

    // 1. 转换到局部UTM坐标
    Eigen::Vector3d local_position = convertToLocalUtm(msg);

    if (!is_initialized_) {
        // 首次获得有效GPS，初始化滤波器状态
        initializeFilter(local_position, msg->header.stamp);
        is_initialized_ = true;
        return;
    }

    // 2. 如果使用EKF，执行更新步
    if (use_ekf_) {
        ekf_->updateGPS(local_position, msg->position_covariance);
    } else {
        // 3. 如果使用图优化，将当前预积分约束和GPS约束添加到图中
        graph_opt_->addGPSConstraint(current_keyframe_id_, local_position, preintegrator_);
        // 触发一次局部优化
        graph_opt_->optimizeWindow();
        // 获取优化后的最新位姿并发布
        auto optimized_pose = graph_opt_->getLatestPose();
        publishFusedPose(msg->header.stamp, optimized_pose.position, optimized_pose.orientation);
        // 重置预积分器，从新的优化后状态开始
        preintegrator_.reset();
    }
}

调试技巧与常见坑点：

1. 可视化是王道：大量使用RViz进行可视化。同时发布原始GPS路径、纯IMU积分路径和融合后路径，直观对比。将GPS的协方差椭圆也可视化出来，能帮助你理解何时GPS不可信。
2. 时间戳对齐检查：在回调函数开头打印msg->header.stamp和ros::Time::now()的差值。如果发现IMU或GPS数据有巨大的延迟，可能是传感器驱动或网络问题。使用rosbag play --clock和use_sim_time参数进行离线回放调试，能复现问题。
3. 协方差调参：EKF或图优化中的过程噪声和观测噪声协方差矩阵（Q和R）极大地影响性能。过程噪声（IMU噪声）调大，滤波器更信任观测；观测噪声（GPS噪声）调大，滤波器更信任预测。一个实用的方法是：在静止状态下，观察融合输出的位置方差是否收敛到一个合理的小值；在匀速直线运动时，观察速度估计是否平滑稳定。
4. 处理初始化：融合系统需要一个良好的初始状态（位置、速度、姿态、零偏）。一个稳健的策略是：车辆静止数秒，用GPS平均值作为初始位置，用加速度计平均值估算重力方向得到初始滚转和俯仰（偏航角可先设为0或由磁力计提供），速度设为0，零偏通过这段时间的IMU数据统计得到。
5. 应对GPS失效：在gpsCallback中，如果连续一段时间（如1秒）没有收到有效GPS信号，应将EKF中GPS观测噪声协方差调至极大，或在图优化中暂时移除GPS因子。同时，可以发布一个std_msgs::Bool话题来指示定位健康状态，供下游模块使用。

调试这样一个系统，耐心比聪明更重要。从静止场景开始，再到匀速直线，最后才测试转弯和加减速。记录完整的ROS bag数据，可以让你反复复现问题，调整参数。当你看到融合后的轨迹在开阔地紧密贴合GPS，在穿过短暂隧道后依然能平滑衔接而不发散时，那种成就感是对所有调试工作的最好回报。