1. 为什么VINS-FUSION里加个GPS反而让轨迹飘得更厉害了？
刚接触VINS-FUSION时，我跟很多同行一样，满心期待把GPS数据一接，定位精度就能“质的飞跃”。结果跑完第一个室外数据集，看着轨迹在地图上画出一道道诡异的波浪线，心里直犯嘀咕：这哪是融合，这简直是互殴。后来翻遍GitHub Issues、ROS论坛和几篇关键论文，才明白问题根本不在代码写错了，而在于——我们压根没搞懂GPS坐标和视觉惯性坐标之间那层薄薄的、却极其致命的“翻译关系”。
VINS-FUSION本身输出的是一个纯视觉-惯性里程计（VIO）的局部坐标系下的位姿，单位是米，原点是第一帧图像的相机光心，Z轴通常指向上（ENU或NED约定下略有不同）。而GPS模块（比如常见的u-blox M8系列）输出的是WGS84大地坐标系下的经纬度和海拔（LLA），单位是度和米，原点是地球质心。这两套坐标系，就像中文和阿拉伯语——语法、单位、参照系全都不一样，直接“拼接”无异于让两个说不同语言的人靠手势谈判，不吵起来才怪。
关键词里虽然没填，但这个项目的核心就三个字：坐标转换。它不是简单的“把经纬度转成XYZ”，而是要完成一个四步闭环：WGS84大地坐标 → ECEF地心地固坐标 → ENU本地东北天坐标 → VIO局部坐标系对齐。每一步都藏着参数陷阱和精度雷区。比如，很多人以为用一个固定的“经纬度转平面坐标”的公式（比如墨卡托投影）就够了，但实际在VINS-FUSION里，ENU坐标系的原点必须严格对应VIO初始化时的第一帧位置，否则整个轨迹都会发生刚体偏移；再比如，GPS的海拔（H）是相对于WGS84椭球面的高度，而VIO的Z轴是相对于局部水平面的高度，两者差了一个“大地水准面高”（geoid height），这个值在不同地区能差出几十米，直接忽略等于给系统喂了一剂强效迷幻药。
我第一次调试时，就是卡在了ENU原点设置上。我把GPS第一帧的经纬度直接当成了ENU原点，结果整条轨迹在地图上整体向西偏移了200多米。后来才发现，VINS-FUSION的pose_graph模块里有个gps_ref参数，它要求你输入的不是原始GPS读数，而是经过高精度RTK解算后、已校正过电离层和对流层延迟的“真实”位置——而我当时用的只是普通单点定位（SPS）数据，误差本身就达5米以上。这就好比你让一个近视500度的人给你指路，还要求他指的方向必须精确到毫米级。
所以，这篇文章不讲怎么编译VINS-FUSION，也不讲IMU标定流程，就死磕这一个点：GPS数据进VINS-FUSION之前，到底经历了哪些不可见的坐标变换？每一步的数学依据是什么？哪些参数可以调，哪些必须死守？实测中哪些坑最致命？这些细节，官方文档一笔带过，论文里藏在附录角落，但它们恰恰是决定你项目成败的“最后一厘米”。
2. WGS84到ENU：四步转换链的数学拆解与物理意义
VINS-FUSION的GPS融合模块（gps_utm或gps_local）背后，是一套严谨的坐标系转换链。它不像手机APP那样“一键转坐标”，而是把地球当作一个精密的椭球体，用一系列可逆的数学映射，把全球唯一的经纬度，一步步“拉平”到你无人机起飞点附近的局部平面上。这四步，缺一不可，顺序也不能乱。下面我用一张表先列出核心步骤，再逐个深挖：
| 步骤 | 输入坐标系 | 输出坐标系 | 核心作用 | 关键参数/陷阱 |
|---|---|---|---|---|
| Step 1 | WGS84 (LLA) | ECEF (X,Y,Z) | 把球面坐标变成三维直角坐标，为后续旋转做准备 | WGS84椭球长半轴a=6378137.0m，扁率f=1/298.257223563 |
| Step 2 | ECEF (X,Y,Z) | ENU (East, North, Up) | 将地心坐标系“旋转+平移”到本地参考点，建立“东-北-天”直角系 | 原点必须是VIO初始化位置，非GPS首帧位置；需高精度LLA |
| Step 3 | ENU (E,N,U) | VIO局部坐标系 (x,y,z) | 对齐两个坐标系的朝向（yaw角）和尺度（scale） | 需求解3自由度刚体变换R,t；常被误认为“只需平移” |
| Step 4 | VIO局部坐标系 | 融合优化变量 | 将GPS观测作为约束项，参与后端图优化 | GPS协方差矩阵必须真实反映定位精度，否则拖垮整个优化 |
现在，我们把镜头推近，看Step 1和Step 2这两个最容易被忽视的环节。
2.1 Step 1：WGS84大地坐标到ECEF坐标的精确映射
WGS84定义了一个标准地球椭球体，其表面任意一点的位置，由纬度φ（phi）、经度λ（lambda）和椭球高h（height above ellipsoid）唯一确定。ECEF坐标系的原点在地球质心，Z轴指向北极，X轴指向本初子午线与赤道交点，Y轴构成右手系。从LLA到ECEF的转换公式是：
N = a / sqrt(1 - e² * sin²(φ)) // 卯酉圈曲率半径 X = (N + h) * cos(φ) * cos(λ) Y = (N + h) * cos(φ) * sin(λ) Z = [N*(1-e²) + h] * sin(φ)其中，a是长半轴（6378137.0 m），e²是第一偏心率平方（约0.00669437999014），N是卯酉圈曲率半径，它随纬度变化——在赤道最大，在两极最小。这个公式看似简单，但实操中三个细节足以让你的轨迹偏移百米：
- 角度单位陷阱：φ和λ必须是弧度制，不是度。我见过太多人直接把GPS输出的“31.234567°”代入公式，结果X、Y坐标直接爆炸。正确做法是
φ_rad = φ_deg * π / 180。 - 高度类型混淆：GPS模块输出的“altitude”字段，绝大多数情况下是椭球高h（height above WGS84 ellipsoid），而非我们日常说的“海拔”（orthometric height，即高于平均海平面的高度）。VINS-FUSION默认按椭球高处理。如果你的GPS模块输出的是正高（比如某些RTK基站配置），就必须减去当地的大地水准面高（geoid separation）来得到h。这个值在中国大部分地区是负值（-10m到-50m），忽略它，Z轴误差直接拉满。
- 精度依赖性：这个转换本身是纯数学的，没有误差。但它的输入——φ, λ, h——的精度，决定了ECEF坐标的上限精度。普通民用GPS的经纬度精度约5米（CEP），换算成ECEF的X/Y/Z误差，会因纬度不同而放大。例如，在北纬40度，1角秒的经度误差≈23米，1角秒的纬度误差≈31米。所以，GPS原始数据的质量，是整个链条的天花板。
2.2 Step 2：ECEF到ENU的“本地化”旋转——原点选择是灵魂
这一步，是VINS-FUSION GPS融合中最常被误解的环节。它的目标，是把以地球质心为原点的ECEF坐标，转换成一个以某个本地点P₀为原点的、符合人类直觉的“东-北-天”坐标系。转换公式是一个标准的旋转矩阵乘法：
[ E ] [ -sin(λ₀) cos(λ₀) 0 ] [ X - X₀ ] [ N ] = [ -sin(φ₀)cos(λ₀) -sin(φ₀)sin(λ₀) cos(φ₀) ] * [ Y - Y₀ ] [ U ] [ cos(φ₀)cos(λ₀) cos(φ₀)sin(λ₀) sin(φ₀) ] [ Z - Z₀ ]其中，(X₀, Y₀, Z₀)是本地原点P₀在ECEF下的坐标，(φ₀, λ₀)是P₀的纬度和经度。
提示：这个公式里的
(φ₀, λ₀)是P₀的地理坐标，而(X₀, Y₀, Z₀)是它通过Step 1公式计算出来的ECEF坐标。二者必须严格对应，不能混用。
那么，P₀该选谁？官方Wiki和多数教程会说：“选GPS数据的第一帧位置”。这是大错特错的！原因有二：
- VIO初始化的不确定性：VINS-FUSION的VIO部分需要几秒时间（通常5-10秒）才能完成初始化，收敛出一个稳定的、尺度正确的位姿。在这段时间内，VIO的轨迹是漂移的、尺度是未知的。如果你把此时的GPS位置（可能本身就有5米误差）设为ENU原点，等于把一个“错误的起点”当作了整个世界的中心。后续所有GPS观测，都会在这个错误的基准上进行匹配，导致全局性偏移。
- GPS与VIO的时间同步偏差：GPS数据和IMU数据存在毫秒级的时间戳偏差。VINS-FUSION内部有时间对齐机制，但如果你强行指定一个“GPS首帧”，而这个帧的时间戳与VIO初始化完成时刻不一致，就会引入额外的刚体偏移。
正确的P₀，必须是VIO初始化完成后，第一个被VINS-FUSION后端确认为“稳定”的VIO位姿所对应的、经过高精度RTK解算的GPS位置。这个位置，通常需要你手动从日志中提取，或者在启动VINS-FUSION前，用一个独立的、高精度的GNSS后处理软件（如RTKLIB）对你的GPS数据进行静态解算，得到一个厘米级精度的“真值”位置，再将其作为gps_ref参数输入。
我做过一个对比实验：用同一段数据，分别以“GPS首帧”和“RTK后处理真值”为P₀。结果前者轨迹在Google Earth上与真实道路偏差达120米，后者偏差仅3.2米。这个差距，不是算法问题，而是“地基”打歪了。
3. ENU到VIO坐标系的刚体对齐：为什么3自由度比6自由度更合理？
完成了WGS84→ECEF→ENU的转换，我们得到了一个以本地点P₀为原点、单位为米的ENU坐标系下的GPS轨迹。但这还不能直接喂给VINS-FUSION。因为VIO自己构建的局部坐标系，其X/Y/Z轴的朝向（orientation）和原点（origin）与ENU系并不一致。它们之间，存在一个未知的刚体变换：一个旋转矩阵R（3×3）和一个平移向量t（3×1）。我们的任务，就是求解这个R和t，让ENU下的GPS点，尽可能“贴合”VIO轨迹上的对应点。
这里就引出了一个关键的设计哲学问题：应该求解完整的6自由度（6-DoF）刚体变换（R + t），还是只求解3自由度（3-DoF）的变换（仅yaw旋转 + t）？VINS-FUSION默认采用的是后者，即假设ENU系和VIO系的X-Y平面是平行的，只存在一个绕Z轴的偏航角（yaw）偏差。这个假设，是工程实践中的一个精妙妥协。
3.1 3-DoF对齐的原理与优势
3-DoF对齐的数学表达非常简洁：
[ x_vio ] [ cos(ψ) -sin(ψ) 0 ] [ E ] [ t_x ] [ y_vio ] = [ sin(ψ) cos(ψ) 0 ]*[ N ] + [ t_y ] [ z_vio ] [ 0 0 1 ] [ U ] [ t_z ]其中，ψ是yaw偏角，(t_x, t_y, t_z)是平移。
这个模型只有4个未知数（ψ, t_x, t_y, t_z），远少于6-DoF的12个。它的合理性基于以下物理事实：
- VIO的roll/pitch精度远高于yaw：视觉-惯性系统通过重力矢量（IMU的z轴）可以非常精确地估计roll（横滚）和pitch（俯仰）角，误差通常在0.1°以内。因此，VIO坐标系的X-Y平面，天然就与ENU系的E-N平面保持了高度平行。强行拟合一个roll/pitch偏差，不仅不会提升精度，反而会因为噪声和模型失配，引入虚假的扭曲。
- Z轴（Up）方向基本一致：ENU的U轴指向天顶（垂直于当地水平面），VIO的Z轴也通过重力校准指向天顶。二者夹角极小，可以忽略。所以
t_z本质上就是两个坐标系原点在垂直方向上的高度差，是一个纯平移量。 - 计算效率与鲁棒性：4个参数的优化，比12个参数快得多，且对异常值（outlier）更鲁棒。在GPS信号偶尔丢失或跳变时，3-DoF模型能更快地恢复稳定。
3.2 如何求解这4个参数：从SVD到RANSAC的实战选择
VINS-FUSION在pose_graph模块中，使用了一种基于SVD（奇异值分解）的闭式解法来求解这4个参数。其核心思想是：先用GPS和VIO在一段时间窗口内的对应点，计算出最优的平移t，然后固定t，再求解最优的yaw角ψ。具体步骤如下：
- 数据对齐：选取一段VIO已稳定、GPS信号良好的时间窗口（建议10-30秒），提取其中N个时间戳对齐的点对：
(E_i, N_i, U_i)和(x_i, y_i, z_i)。 - 计算质心：求GPS点的质心
(E_c, N_c, U_c)和VIO点的质心(x_c, y_c, z_c)。 - 中心化：将所有点减去各自质心，得到中心化后的坐标
(ΔE_i, ΔN_i, ΔU_i)和(Δx_i, Δy_i, Δz_i)。 - 构造协方差矩阵：对于X-Y平面，构造2×2矩阵
H = Σ(ΔE_i, ΔN_i)^T * (Δx_i, Δy_i)。 - SVD分解：对H进行SVD分解，
H = U * S * V^T，则最优旋转矩阵R = V * U^T。由于我们只允许yaw旋转，R必然是[cosψ, -sinψ; sinψ, cosψ]的形式，从中即可解出ψ。 - 计算平移：
t = [x_c, y_c, z_c]^T - R * [E_c, N_c, U_c]^T。
注意：这只是一个理论流程。在VINS-FUSION的实际代码中（
pose_graph/gps_utm.cpp），它使用了更鲁棒的RANSAC（随机抽样一致性）算法来处理GPS中的粗差（gross error）。RANSAC会随机选取若干点对，计算一组R,t，然后统计有多少个点对的重投影误差小于阈值（如2米），迭代多次后，选择内点（inlier）最多的那组R,t作为最终结果。这是应对城市峡谷中GPS多路径效应的关键。
我在调试一个车载数据集时，发现单纯用SVD，轨迹在高楼间会出现周期性抖动。启用RANSAC后，抖动完全消失。原因就是RANSAC自动剔除了那些被高楼反射、导致定位跳变的GPS坏点。
4. VINS-FUSION源码级解析：gps_utm模块的参数配置与避坑指南
理论讲完，现在进入最硬核的部分：如何在VINS-FUSION的源码和配置文件中，把上述所有数学逻辑落地？这一步，是连接“纸上谈兵”和“实车跑通”的桥梁。我将以VINS-FUSION的config/pose_graph/目录下的vins_config.yaml和gps_utm.cpp源文件为蓝本，逐行拆解。
4.1 配置文件vins_config.yaml中的GPS相关参数
在vins_config.yaml中，与GPS融合直接相关的参数集中在pose_graph节点下。以下是每个参数的真实含义和常见误用：
pose_graph: # --- 关键参数：GPS参考点 --- gps_ref: [31.234567, 121.456789, 5.2] # 必须！是WGS84经纬度+椭球高（单位：度,度,米） # 错误示范：填入"0,0,0"或"GPS首帧数据" # 正确做法：用RTKLIB对GPS数据做静态解算，取结果均值 # --- GPS数据输入格式 --- gps_type: 1 # 0=LLA格式（经度,纬度,海拔），1=UTM格式（东距,北距,海拔） # 大多数GPS模块输出LLA，所以填1是错的！应填0 # 填错会导致坐标系彻底混乱，轨迹飞天 # --- 坐标转换与对齐 --- gps_local: true # true=启用ENU到VIO的3-DoF对齐；false=强制使用gps_ref作为绝对原点 # 推荐true，尤其在VIO初始化不稳定时 gps_local_window: 30 # 对齐所用的时间窗口长度（秒）。太短（<10）易受噪声影响；太长（>60）会模糊动态特性 # --- 融合权重与鲁棒性 --- gps_noise: [10.0, 10.0, 5.0] # GPS观测的协方差矩阵对角线元素（单位：米²） # 对应[E,N,U]三个方向。必须真实！ # 错误：全填1.0（以为越小越好）→ 系统过度信任GPS，压制VIO，导致轨迹僵硬、失去灵活性 # 正确：根据你的GPS模块规格书填写。例如u-blox M8N单点定位，填[25.0, 25.0, 15.0] gps_ransac_threshold: 2.0 # RANSAC算法的内点判定阈值（米）。城市环境填1.5-2.0；开阔地可填3.0-5.0提示：
gps_ref参数是整个GPS融合的“锚点”。我曾见过一个项目，因为gps_ref填错了小数点（31.234567写成312.34567），导致整条轨迹在地图上显示在太平洋中央。这种低级错误，往往要花半天时间排查。
4.2 源码gps_utm.cpp中的核心逻辑与调试技巧
VINS-FUSION的GPS处理逻辑，主要封装在pose_graph/gps_utm.cpp中。理解它的执行流程，是高级调试的必备技能。其主干逻辑如下：
- 数据接收与预处理(
GPSHandler::process)：监听/gps/fix话题，将sensor_msgs/NavSatFix消息解析为double lat, lon, alt。这里有一个隐藏陷阱：alt字段的含义取决于status.status。如果status.status == -1（无效），alt可能是0或NaN，必须过滤。 - WGS84→ECEF转换(
wgs84_to_ecef)：调用标准公式。注意，此函数内部会检查lat, lon是否在有效范围内（-90~90, -180~180），若超限，会返回(0,0,0)，导致后续所有计算失效。 - ECEF→ENU转换(
ecef_to_enu)：使用gps_ref计算X₀,Y₀,Z₀和φ₀,λ₀，然后应用旋转矩阵。关键点来了：这个函数内部会检查gps_ref是否已被设置。如果未设置（即gps_ref[0]==0 && gps_ref[1]==0），它会静默地使用当前GPS点作为临时原点！这就是为什么很多人不填gps_ref也能跑，但结果完全不准——系统在“自作主张”。 - ENU→VIO对齐(
PoseGraph::solveGps)：执行RANSAC+SVD流程。调试时，可以在solveGps函数末尾添加日志，打印出每次计算出的ψ, t_x, t_y, t_z，观察它们是否在合理范围内波动（ψ应在±10°内，t_x/t_y应在±5米内）。
实战技巧：在
CMakeLists.txt中，为vins_estimatortarget添加编译选项-DDEBUG_GPS，然后在gps_utm.cpp中用#ifdef DEBUG_GPS ... #endif包裹关键日志。这样，你可以在不修改主逻辑的情况下，获得详细的GPS处理流水线日志，精准定位是哪一步出了问题。
5. 实测案例复盘：从轨迹发散到厘米级精度的完整排错链路
理论和配置都讲完了，最后，我用一个真实的、耗时三天的排错案例，带你走一遍从“轨迹像喝醉”到“稳如泰山”的全过程。这个案例，涵盖了上面提到的所有坑，极具代表性。
5.1 问题现象：轨迹在Google Earth上呈现规律性“S形”偏移
某次在大学校园内测试一台搭载VINS-FUSION的无人机。硬件：Intel RealSense D435i（IMU+RGB-D）、u-blox M8N GPS模块。数据采集后，用rosbag回放，rviz中看到VIO轨迹（绿色）和GPS轨迹（红色）在前10秒还能勉强重合，之后GPS轨迹开始缓慢右偏，到60秒时，偏移已达15米，并呈现出明显的正弦波形态。导出KML文件到Google Earth，发现整条轨迹像一条被风吹弯的丝带。
5.2 排查链路：从表象到根因的五步法
Step 1：确认GPS数据质量
首先，用gps_common包的gps_analyzer.py脚本分析原始/gps/fix话题。发现HDOP（水平精度因子）在2.0-8.0之间剧烈跳变，平均值高达4.5。这意味着GPS信号受到了严重多路径干扰（校园内高楼林立）。结论：GPS原始数据质量差，是问题的源头之一，但无法解释“S形”规律。
Step 2：检查gps_ref设置
打开vins_config.yaml，发现gps_ref被注释掉了！程序正在使用默认的[0,0,0]。立刻用RTKLIB对GPS数据进行静态解算，得到真值[31.234567, 121.456789, 5.2]，填入配置。重启，轨迹整体偏移减小到8米，但“S形”依然存在。说明gps_ref是必要条件，但非充分条件。
Step 3：验证坐标系转换
在gps_utm.cpp中添加日志，打印wgs84_to_ecef的输出。发现Z坐标在-6370000到-6369000之间跳变，而正常ECEF的Z坐标应在-6370000左右。进一步检查，发现alt字段在GPS信号弱时，会输出一个极大负值（如-999999.0），wgs84_to_ecef函数没有对此做保护，直接参与了计算。Bug定位成功！在process函数中加入判断：
if (gps_msg.altitude < -1000.0 || gps_msg.altitude > 10000.0) { ROS_WARN("Invalid GPS altitude: %f, skipping", gps_msg.altitude); return; }修复后，“S形”的幅度减小了一半。
Step 4：分析RANSAC对齐结果
开启DEBUG_GPS，观察solveGps的日志。发现ψ（yaw角）在-5°到+15°之间无规律跳变，而gps_local_window设为30秒，理论上应该很稳定。怀疑是时间窗口内包含了GPS信号剧烈跳变的点。将gps_local_window从30改为10，并将gps_ransac_threshold从2.0降低到1.2。重启，ψ的波动范围收窄到±2°，轨迹的“S形”几乎消失。
Step 5：校准GPS协方差
最后，检查gps_noise。配置中是[1.0, 1.0, 1.0]，这显然过于乐观。查阅u-blox M8N手册，其单点定位的95%置信度精度为2.5m（H）/5.0m（V）。将其换算为标准差（σ），并平方得到协方差：[ (2.5/1.96)^2, (2.5/1.96)^2, (5.0/1.96)^2 ] ≈ [1.6, 1.6, 6.5]。更新配置，重启。最终，轨迹与真实道路的偏差稳定在2.3米以内，达到了预期目标。
5.3 经验总结：三条铁律
这次排错，让我总结出三条必须刻在脑子里的铁律：
- 铁律一：GPS数据质量是地基，地基不牢，一切白搭。永远不要相信“能收到信号”就等于“数据可用”。必须用专业工具分析HDOP、PDOP、卫星数量、信噪比（SNR）。
- 铁律二：
gps_ref不是可选项，而是必填项，且必须是“真值”。宁可花半天用RTKLIB做后处理，也不要图省事填GPS首帧。 - 铁律三：所有参数都不是“设了就行”，而是“设得准才行”。
gps_noise不是调参游戏，而是对传感器物理特性的忠实建模；gps_ransac_threshold不是越大越好，而是要与你的应用场景（城市/旷野）匹配。
6. 进阶思考：当GPS信号完全丢失时，VINS-FUSION如何维持精度？
聊完GPS融合的细节，一个自然的问题浮现：如果无人机飞进了隧道、地下车库，或者遭遇了强烈的电磁干扰，GPS信号完全中断，VINS-FUSION还能工作吗？答案是肯定的，但这恰恰暴露了VIO与GPS融合的本质关系——GPS不是VIO的“老师”，而是它的“校准器”和“锚点”。
VINS-FUSION的VIO部分，是一个完全自主的、不依赖外部信号的导航系统。它依靠相机的视觉特征跟踪和IMU的加速度/角速度积分，来实时估计自身的运动。在GPS信号良好时，GPS的作用是：
- 提供绝对尺度：解决VIO固有的尺度不确定性（scale drift）。
- 提供绝对位置：将VIO的局部漂移轨迹，锚定到全球坐标系中。
- 提供长期稳定性：抑制VIO随时间累积的漂移。
一旦GPS丢失，VIO并不会“崩溃”，而是会退回到其固有的性能模式：短期精度极高（<10秒内，误差<0.5米），但长期会漂移（>60秒，误差可能达数米甚至数十米）。这个漂移，主要来自IMU的零偏（bias）和温漂（temperature drift）。
所以，一个健壮的系统设计，必须考虑GPS的“降级模式”。VINS-FUSION本身没有内置的GPS丢失检测与平滑切换逻辑，这需要你在上层应用中实现。我的做法是：
- 实时监控GPS状态：订阅
/gps/fix的status.status和position_covariance。当status.status == -1（无效）或position_covariance[0] > 100.0（协方差过大）持续3秒以上，即判定为GPS丢失。 - 触发VIO重初始化：向VINS-FUSION的
/vins_estimator/restart服务发送请求，强制其丢弃当前的GPS锚点，重新以丢失时刻的VIO位姿为新的局部原点，开始纯VIO导航。这能避免尺度突变。 - 启用IMU预积分补偿：在
vins_config.yaml中，确保imu_propagation为true，并适当增大max_imu_interval（如从0.01改为0.05），让系统在IMU数据稀疏时，也能更稳健地进行状态传播。
有一次，我们在一个废弃工厂内部测试，GPS信号在入口处就消失了。靠着这套降级策略，VINS-FUSION在纯VIO模式下，成功完成了长达90秒、总长约120米的自主导航，最终出口处的定位误差仅为1.8米。这个结果，证明了VIO本身的强大，也印证了GPS融合的真正价值——不是让它“一直在线”，而是让它在关键时刻，为我们“扶一把”。
最后分享一个小技巧：在rviz中，同时加载/vins_estimator/odometry（VIO）和/vins_estimator/gps_odometry（融合后）两个话题，并将它们的Fixed Frame都设为world。当GPS正常时，两条轨迹严丝合缝；当GPS丢失时，你会清晰地看到融合轨迹（绿色）开始缓慢偏离VIO轨迹（蓝色），这个视觉反馈，比任何日志都直观。
