简介本资源是一份面向自动驾驶与机器人领域研究人员及工程师的Fast-LIO算法深度解析与工程改进文档聚焦多传感器融合建图与定位中的关键瓶颈——点云特征缺失导致的建图失败问题。作者在原始Fast-LIO融合IMU与激光雷达基础上创新性引入GPS数据完成坐标系对齐标定、姿态矫正与联合状态优化显著提升复杂场景下的鲁棒性与定位精度。文档涵盖算法目的、完整流程含数据时间对齐、前向/反向传播、KD树建图、观测方程设计与偏移量优化、核心数学推导位置/旋转/速度更新、李代数运算、误差传播协方差建模及源码级改进细节内容兼具理论深度与实践指导性。资源为1个756KB的Word文档.docx结构清晰含详细目录与公式推导过程适合具备SLAM基础、熟悉线性代数与概率论的进阶学习者系统掌握Fast-LIO原理与融合扩展方法。目前已有437人学习下载。1. Fast-LIO 不是“快一点的 LOAM”而是用 IMU 把点云时间轴焊死为什么它能在 mid360 雷达上跑出 100Hz 级建图却对 GPS 坐标系异常敏感Fast-LIO 的“快”从来不是靠删减优化项换来的——它把传统 SLAM 中最耗时的帧间匹配ICP、NDT整个砍掉转而用 IMU 构建一条刚性时间轴让每一帧点云中数万个点不再被当作一个整体去配准而是被拆解成带精确时间戳的独立粒子在统一的运动学模型下各自回溯到世界坐标系。这才是它能在 mid360单帧约 2 万点、10Hz 扫描上实现实时建图的底层逻辑。但这个设计也埋下了一个关键矛盾IMU 提供的是相对位姿演化其零点漂移和尺度不确定性会随时间累积而 GPS 提供的是绝对地理坐标精度高但更新慢通常 1–10Hz、噪声大、易受遮挡。当把 GPS 强行“缝合”进 Fast-LIO 的状态向量时不是简单加个观测方程就能提升精度——它会像往高速旋转的陀螺里塞进一块磁铁瞬间扰动整个前向传播链的数值稳定性。本文要讲清楚的正是这个“缝合”动作背后必须直面的三重机制冲突时间尺度失配IMU 5ms/帧 vs GPS 100ms/帧、坐标系语义错位IMU 的 body-frame 是运动学载体GPS 的 WGS84 是大地测量载体、以及误差传播路径的不可逆性GPS 偏差一旦注入前向传播会在后续所有点云反向传播中被指数级放大。这不是一个“加个传感器”的工程问题而是一个关于状态空间定义权的算法哲学问题你到底想让系统相信谁的时间谁的原点谁的尺度搞懂这三点才能真正看懂作者在源码里改的那几行 GPS 融合逻辑为什么既不是玄学调参也不是黑匣子补丁而是一次对 Fast-LIO 底层状态机的外科手术式重构。2. 时间对齐与前向传播IMU 不是辅助传感器而是点云的“时间编译器”Fast-LIO 的实时性根基不在后端优化而在前端——它把 IMU 当作一个连续时间微分方程求解器为每一帧稀疏点云中的每个点赋予亚毫秒级的精确时空坐标。这彻底颠覆了传统激光 SLAM “帧对帧配准”的范式。本章将从数据流源头切入拆解 IMU 如何在数学上“编译”点云时间并揭示前向传播中那些看似平平无奇的公式实则承载着对刚体运动学最严苛的约束。2.1 数据时间对齐不是插值而是构建 IMU 时间窗口的“事件驱动”机制mid360 雷达每 0.1s 输出一帧点云而 IMU 数据以 200Hz5ms/帧持续涌入。Fast-LIO 并不采用简单的线性插值将 IMU 姿态“贴”到雷达时间戳上。它的核心策略是为每一帧雷达点云划定一个专属的 IMU 时间窗口并确保该窗口内所有 IMU 测量都参与当前帧的位姿演化计算。# 源码中典型的 IMU 时间窗口构建逻辑伪代码 def align_imu_to_lidar(lidar_timestamp, imu_buffer): # imu_buffer 是按时间排序的 IMU 数据队列 # 找到第一个 IMU 时间戳 lidar_timestamp - 0.05s即窗口左边界 start_idx bisect_left(imu_buffer.timestamps, lidar_timestamp - 0.05) # 找到最后一个 IMU 时间戳 lidar_timestamp 0.05s即窗口右边界 end_idx bisect_right(imu_buffer.timestamps, lidar_timestamp 0.05) # 关键窗口大小固定为 ~20 帧对应 0.1s而非动态伸缩 # 这保证了每次前向传播的计算量恒定是实时性的基石 window_size min(20, end_idx - start_idx) return imu_buffer[start_idx:start_idx window_size]提示这个“20 帧”不是经验值而是由lidar_timestamp的采样周期Δt_lidar 0.1s和 IMU 周期Δt_imu 0.005s严格决定的Δt_lidar / Δt_imu 20。它意味着 Fast-LIO 的每一次forward_propagation()调用处理的都是恰好覆盖一帧雷达扫描全过程的 IMU 运动学轨迹。这解释了为何它能规避 ICP 的迭代收敛耗时——点云本身已被 IMU 的时间轴“拉直”了。2.2 前向传播的状态向量为什么R_I和R_L必须分离且R_L被设为常量Fast-LIO 的状态向量x [P_I, R_I, R_L, T_L, v_I, b_w, b_a, g]表面看是堆砌实则每一项都对应一个物理可解耦的自由度P_I,R_I: IMU 坐标系即系统主坐标系相对于初始帧的全局位置与朝向。这是待估的核心状态。R_L,T_L: 雷达坐标系相对于 IMU 坐标系的外参旋转与平移。它们是刚性安装带来的固定偏移在运行时绝不更新。v_I: IMU 在全局坐标系下的瞬时线速度由加速度积分得到。b_w,b_a: 陀螺仪与加速度计的零偏。源码中注释为“未启用”因其在 Fast-LIO 的轻量级设计中被简化为常量或通过离线标定预补偿。g: 重力向量。其方向由初始静止状态标定大小固定为9.8 m/s²。这种分离设计的深层动机在于解耦运动学与几何标定R_I描述的是 IMU 自身的运动它必须随时间演化R_L描述的是传感器间的静态几何关系它只应在系统启动时标定一次。若在运行时将其纳入状态并更新等于允许雷达“在自己身上晃动”这会直接污染所有点云的反向传播结果导致地图发散。2.3 旋转矩阵的李代数推导R_{i1} R_i * exp((ω_m - b_w)^ * Δt)为什么不能写成R_i ...这是理解 Fast-LIO 数值稳定性的核心。旋转矩阵R属于特殊正交群SO(3)它不构成向量空间因此不能进行普通加减法。强行写R_{i1} R_i ΔR会导致R_{i1}迅速失去正交性几轮迭代后行列式偏离 1姿态爆炸。正确的微分方程是dR/dt R * ω^其中ω^是角速度ω的反对称矩阵。其解析解为R(t) R(t₀) * exp(∫_{t₀}^t ω(τ)^ dτ)对离散 IMU 数据使用一阶近似∫_{t_i}^{t_{i1}} ω(τ)^ dτ ≈ (ω_m,i - b_w)^ * Δt故得R_{i1} R_i * exp((ω_m,i - b_w)^ * Δt)exp(·)是SO(3)上的指数映射它天然保证了结果仍是旋转矩阵。源码中调用的Sophus::SO3d::exp()或Eigen::AngleAxisd正是此映射的实现。忽略这一点任何基于R的数值微分如雅可比计算都会失效。2.4 速度与位置的耦合更新为什么v_I的初始化为 0 是安全的而P_I却不能速度更新公式v_{I,i1} v_{I,i} Δt * R_I,i * (a_m,i - b_a) Δt * g位置更新公式P_{I,i1} P_{I,i} Δt * v_{I,i}这里的关键在于初值敏感性v_I是一个瞬时量其初值v_{I,0} 0对应系统启动时静止假设。即使有小偏差也会被后续 IMU 加速度积分快速修正因加速度测量信噪比远高于速度。P_I是一个累积量其初值P_{I,0} 0定义了整个建图的原点。这个原点一旦设定就成为所有点云坐标的基准。Fast-LIO 默认将第一帧 IMU 的位置设为(0,0,0)这在纯里程计模式下完全合理但当你引入 GPS 时这个(0,0,0)就必须被强制对齐到 GPS 的(lat, lon, alt)对应的 ECEF 或 UTM 坐标否则整个地图会漂移出地理坐标系。2.5 前向传播的协方差传播为什么P_{i1} F_x * P_i * F_x^T F_w * Q * F_w^T中的F_x是块对角稀疏的状态误差δx_i x_i - x̂_i的协方差P_i E[δx_i δx_i^T]的传播依赖于状态转移雅可比F_x ∂f/∂x和噪声雅可比F_w ∂f/∂w。f(x, u, w)是前向传播的动力学函数。F_x的结构揭示了状态变量间的弱耦合性∂P_I/∂R_I 0位置更新不显式依赖当前旋转v_I是独立状态。∂R_I/∂v_I 0旋转更新只依赖角速度与线速度无关。∂v_I/∂R_I ≠ 0加速度在全局坐标系下的投影R_I * a显式依赖R_I这是F_x中唯一非零的跨块项。因此F_x是一个24×248 状态 × 3 维的块矩阵其非零块集中在R_I → R_I李代数扰动、R_I → v_I旋转影响加速度投影、v_I → P_I速度影响位置这三个位置。这种稀疏性是 Fast-LIO 能高效计算协方差的基础也是后续卡尔曼更新中H_k矩阵结构的源头。3. 反向传播与 KD 树建图点云不是“图像”而是 IMU 运动轨迹上的“时空采样点”在 Fast-LIO 中一帧点云绝非一个静态的几何快照。它是雷达在0.1s扫描时间内沿着 IMU 描绘的连续运动轨迹所采集的一系列离散空间点。反向传播的本质就是将这些点沿轨迹“倒放”精确还原它们在世界坐标系中的真实位置。这一步的精度直接决定了后续 KD 树建图的质量和最终定位的鲁棒性。3.1 反向传播从“点云帧”到“点云粒子”的解构对于雷达点云中的任意一点p_j在雷达坐标系L下其扫描时刻t_j必然落在某个 IMU 时间窗口[t_i, t_{i1}]内。反向传播分三步时间戳对齐找到t_j左侧最近的 IMU 帧i其姿态为x_i [P_I,i, R_I,i, ...]。局部坐标系转换将p_j从雷达坐标系L转到 IMU 坐标系Ip_I,j R_L * p_j T_L # R_L, T_L 是已知外参全局坐标系投影利用x_i的位姿将p_I,j投影到世界坐标系W即第一帧 IMU 坐标系p_W,j R_I,i * p_I,j P_I,i注意这里没有使用R_I,i和P_I,i的插值而是直接取最近的 IMU 帧。这是因为 Fast-LIO 的 IMU 频率足够高200HzΔt 5ms的量化误差远小于点云的空间分辨率mid360 角度分辨率约 0.05°在 10m 处对应约 8mm可忽略不计。这种“取整”策略极大简化了计算是实时性的又一保障。3.2 KD 树建图为什么是“体素 KD 树”而不是标准 KD 树标准 KD 树对点云建模存在两个致命缺陷内存爆炸mid360 单帧 2 万点100 帧即 200 万点标准 KD 树节点数与点数同阶内存占用巨大。近邻搜索低效在稠密区域大量点落入同一叶节点搜索时需遍历所有点。Fast-LIO 采用体素滤波 KD 树的混合结构先将世界坐标系下的所有点p_W,j投入一个三维体素网格如1m × 1m × 1m。每个体素内只保留距离体素中心最近的那个点作为代表。将所有体素代表点构建 KD 树。// C 伪代码体素滤波核心逻辑 struct VoxelKey { int x, y, z; bool operator(const VoxelKey other) const { /* ... */ } }; std::mapVoxelKey, Point3D voxel_map; for (const auto point : world_points) { // 计算体素索引向下取整 int vx static_castint(point.x / voxel_size); int vy static_castint(point.y / voxel_size); int vz static_castint(point.z / voxel_size); VoxelKey key{vx, vy, vz}; // 计算到体素中心的距离 double cx (vx 0.5) * voxel_size; double cy (vy 0.5) * voxel_size; double cz (vz 0.5) * voxel_size; double dist_sq (point.x-cx)*(point.x-cx) (point.y-cy)*(point.y-cy) (point.z-cz)*(point.z-cz); // 如果体素为空或当前点更近则更新 if (voxel_map.find(key) voxel_map.end() || dist_sq voxel_map[key].dist_sq) { voxel_map[key] {point, dist_sq}; } } // 将 voxel_map 中的所有 point 构建 KD 树这种设计将点云规模压缩了 10–100 倍取决于体素大小同时保证了地图的几何完整性——体素代表点足以描述环境的主要轮廓为后续平面拟合提供足够支撑。3.3 观测方程与平面拟合为什么“选 5 个最远点”比“选 10 个最近点”更鲁棒观测方程h_j(x_k, f_j) n_j^T * p_G,kj 0的目标是让当前帧点p_j在世界坐标系下的投影p_G,kj落在由 KD 树中邻近点拟合出的平面上。Fast-LIO 的平面拟合流程是对p_G,kj在 KD 树中搜索K10个最近邻点。计算这 10 个点到p_G,kj的欧氏距离选取距离最大的 5 个点。对这 5 个点用 SVD 求解最优平面n_j^T * X d_j 0。计算这 5 个点到该平面的距离若最大距离 0.1m则判定该点“不成平面”丢弃。这个“选最远点”的策略是针对点云边缘效应的精妙设计在物体边缘如墙角、桌沿最近的几个点可能来自不同平面一面墙和地面强行拟合会得到一个无意义的“平均平面”。而距离最远的 5 个点更可能来自同一个、更稳定的平面如整面墙壁它们的空间分布更广拟合出的法向量n_j更准确、更抗噪。这是一种主动放弃局部细节、拥抱全局结构的鲁棒性设计与视觉 SLAM 中 ORB 特征点的“非极大值抑制”异曲同工。3.4 观测雅可比H_kj的物理意义为什么只有∂h/∂P_I和∂h/∂R_I非零观测方程h_j n_j^T * (R_I,k * (R_L * p_j T_L) P_I,k)对状态x_k的雅可比H_kj ∂h_j/∂x_k^T其结构为H_kj [ n_j^T, -n_j^T * R_I,k * R_L * p_j^, 0, 0, 0, 0, 0, 0 ] [ 1x3 1x3 (李代数扰动) 7x3 zeros ]其中p_j^是p_j的反对称矩阵。这揭示了观测信息的物理局限性∂h/∂P_I n_j^T平面法向量直接给出了位置误差在法线方向上的投影。这是最直接的观测。∂h/∂R_I -n_j^T * R_I,k * R_L * p_j^点p_j相对于 IMU 的位置R_L * p_j T_L经旋转后其在法向量上的投影会随R_I变化。p_j^体现了该点的“杠杆臂”效应——离 IMU 越远的点其姿态误差对观测的影响越大。其余项∂h/∂R_L,∂h/∂T_L,∂h/∂v_I等全为零因为R_L和T_L是已知常量v_I不参与观测方程的计算。这意味着Fast-LIO 的观测只能校正P_I和R_I对其他状态如b_w,b_a毫无约束。这也是为什么 IMU 零偏必须靠离线标定或长时间静止估计无法仅靠激光观测在线收敛。4. GPS 融合的避坑指南当“绝对坐标”撞上“相对运动学”这 4 个坑踩一个就崩将 GPS 数据融入 Fast-LIO绝非在main()函数里加一行gps_callback()那么简单。它是一场在时间、空间、尺度三个维度上与系统底层机制的硬碰硬。以下是作者在源码改进过程中用血泪经验总结出的 4 个高频翻车点每一个都对应一个具体的现象 → 原因 → 解决链路。4.1 现象地图整体缓慢漂移GPS 坐标越跟越歪最后完全错位原因GPS 坐标系WGS84与 Fast-LIO 的世界坐标系通常是 ENU 或自定义的局部坐标系未做严格对齐。直接将(lat, lon, alt)当作(x, y, z)输入忽略了地球曲率和投影变形。在 1km 范围内WGS84 经纬度的 1e-6 度误差就对应约 10cm 的平面偏移且该偏移随地理位置非线性变化。解决必须在系统启动时用至少 3 个已知 GPS 位置的静态点解算一个WGS84 → ENU的 Helmert 七参数变换3 平移 3 旋转 1 尺度。开源库pyproj或geographiclib可完成此转换。切记alt海拔必须转换为h椭球高或H正高并统一到与 IMU 高度基准一致的参考面如 EGM96。4.2 现象加入 GPS 后建图帧率骤降 50%CPU 占用飙升甚至触发 watchdog原因GPS 更新频率1–10Hz远低于 IMU200Hz和雷达10Hz。若将 GPS 观测作为与激光观测同等权重的“帧级”输入每次 GPS 到来时都会触发一次完整的卡尔曼更新含H_k计算、矩阵求逆其计算复杂度O(n³)会拖垮实时性。解决采用异步观测融合策略。GPS 观测不参与forward_propagation而是在其到达时单独构造一个gps_observation并设计一个轻量级的gps_update()函数。该函数只更新与 GPS 直接相关的状态P_I,R_I的 yaw并跳过对v_I、b_w等无关状态的更新。其H_gps矩阵仅为3x24仅对P_I有 3 个非零元对R_I的 yaw 角有 1 个非零元大幅降低计算量。4.3 现象GPS 信号短暂丢失如隧道恢复后地图出现剧烈抖动或撕裂原因GPS 丢失期间系统完全依赖 IMU 积分零偏和噪声导致位姿快速发散。当 GPS 信号恢复一个带有较大偏差的绝对位置观测被强行注入与当前高度发散的P_I状态产生巨大残差z_k - h(x_k)卡尔曼增益K会试图用一个巨大的Δx来修正从而引发抖动。解决引入观测置信度门控。为每个 GPS 观测附加一个accuracy字段如 HDOP 值。在gps_update()中根据accuracy动态调整观测噪声协方差R_gps// R_gps 初始化为对角阵 [σ_x², σ_y², σ_z²] double sigma_scale std::max(1.0, gps_accuracy_m / 1.0); // 以 1m 为基准 R_gps.diagonal() * (sigma_scale * sigma_scale);当accuracy很差时R_gps变大K自动变小GPS 观测的修正力度减弱系统更信任 IMU 的短期预测从而平滑过渡。4.4 现象在开阔地带 GPS 精度很高但建图效果反而不如纯 IMU雷达模式原因GPS 的z高度分量精度通常 5–10m远低于x,y0.5–3m。当将 GPS 的(x,y,z)作为一个整体观测向量输入时低精度的z会通过卡尔曼滤波的耦合效应污染高精度的x,y估计导致平面定位精度下降。这是一种典型的“木桶效应”。解决实施分量级观测选择。在gps_update()中只使用x,y分量构造观测向量z_gps [x_gps, y_gps]并设置对应的H_gps [I_2x2, 0_2x22]。完全忽略z分量让高度信息继续由 IMU 积分和激光平面观测共同约束。这需要修改状态向量定义将P_I的z分量与x,y解耦或在观测时进行投影。5. 状态优化与卡尔曼滤波为什么 Fast-LIO 用贝叶斯融合而不是直接上 L-MFast-LIO 的后端优化表面看是列文伯格-马夸尔特L-M法实则其核心是贝叶斯框架下的卡尔曼滤波。它不追求全局最优而是在 IMU 的先验运动学模型与激光/GPS 的似然观测模型之间寻找一个计算代价最低、数值最稳定的后验估计。本章将揭示这一设计背后的三重取舍实时性、鲁棒性与可解释性。5.1 贝叶斯融合的数学本质P(x|A,B) ∝ P(x|A) * P(x|B)为何是唯一解文中推导的P_AB (P_A^{-1} P_B^{-1})^{-1}和μ_AB P_AB * (P_A^{-1} * μ_A P_B^{-1} * μ_B)并非一个技巧而是高斯分布下贝叶斯推理的必然结果。当两个独立观测AIMU 预测和B激光/GPS 观测都服从高斯分布时其联合后验分布P(x|A,B)必然也是高斯分布且其均值与协方差由上述公式唯一确定。这带来一个关键优势可增量更新。系统无需存储所有历史观测只需维护当前的后验P(x_k)和μ_k。当新观测B_{k1}到来时直接用P(x_k)作为新的先验P_A与B_{k1}的似然P(x|B_{k1})进行贝叶斯融合即可得到P(x_{k1})。这与 L-M 需要构建并求解大型 Hessian 矩阵H J^T * Ω * J的全局优化范式有着根本区别。5.2 卡尔曼增益K的物理含义为什么K P * H^T * (H * P * H^T R)^{-1}是最优的K是一个24x3状态维 × 观测维的矩阵它定义了“观测信息”如何按比例分配给各个状态变量。其最优性体现在最小化后验估计的均方误差E[(x - μ_AB)^2]。从公式看K的大小由三者决定P先验不确定性。P越大先验越不准K越大系统越相信新观测。H观测对状态的敏感度。H的某一行越大说明该观测对对应状态越敏感K中相应列的权重越高。R观测噪声。R越大观测越不可靠(H*P*H^T R)^{-1}越小K整体越小系统越保守。在 Fast-LIO 中H的结构仅P_I和R_I有贡献与R的设置激光观测R_laser小GPSR_gps大共同塑造了K的行为它会优先、强力地修正位置和朝向而对速度、零偏等状态几乎不施加影响。这是一种符合物理直觉的、有层次的修正策略。5.3 SMW 恒等式的实践价值如何避免O(n³)的矩阵求逆P_AB (P_A^{-1} P_B^{-1})^{-1}的直接计算需要对24x24矩阵求逆复杂度O(24³) 13824。而 SMW 恒等式提供了另一种形式P_AB P_A - P_A * H^T * (H * P_A * H^T R)^{-1} * H * P_A其中H * P_A * H^T R是一个3x3矩阵激光观测或2x2矩阵GPSx,y观测其求逆复杂度仅为O(27)或O(8)。虽然多了一次24x3和3x24的矩阵乘法O(24*3*24)1728但总计算量远低于直接求逆。Fast-LIO 的源码正是采用此形式。它将高昂的24x24求逆降维到廉价的3x3求逆这是其实现实时性的关键数值技巧。任何试图绕过 SMW、直接操作大协方差矩阵的“改进”都会在性能上付出惨重代价。5.4 L-M 与卡尔曼的抉择为什么 Fast-LIO 不用 L-M 做全局优化L-M 法的优势在于能跳出局部极小值获得全局更优解。但它有三大硬伤与 Fast-LIO 的设计哲学相悖计算不可预测L-M 的迭代次数取决于初始猜测和地形无法保证单帧处理时间上限违反实时性硬约束。内存开销巨大需要存储J雅可比矩阵3N x 24N为有效观测点数在 mid360 下N≈1000J占用内存超3MB且需频繁分配释放。数值不稳定H J^T * Ω * J是病态矩阵λ阻尼因子的选取是玄学调试困难。Fast-LIO 选择卡尔曼是用“次优但确定”的后验估计换取“实时、稳定、可预测”的系统行为。它承认 IMU 和激光观测的模型是近似的不追求理论最优而追求在各种工况下都能给出一个“足够好且不崩溃”的结果。这是一种面向工程落地的务实选择。6. 设计取舍的终极思考当 GPS 成为“锚点”Fast-LIO 还是 Fast-LIO 吗写到这里我必须坦白一个在无数次调试中浮现的、近乎悖论的疑问当我们把 GPS 作为绝对锚点强行将 Fast-LIO 的世界坐标系W与地理坐标系G对齐时我们究竟是在增强它还是在解构它Fast-LIO 的灵魂在于其纯粹的相对运动学。它的W坐标系是一个由第一帧 IMU 定义的、无物理意义的数学原点。这个原点可以是车库地板上的任意一点只要它能为后续所有点云提供一个一致的参考系统就能完美工作。GPS 的引入却把这个原点强行绑定到了地球椭球体上的一个经纬度。这带来了两重根本性改变第一重是时间语义的冲突。IMU 的t0是系统上电时刻GPS 的t0是 UTC 时间戳。二者通过 NTP 或 PPS 信号同步但同步误差本身就是一个10–100ms量级的噪声源。这个噪声会直接转化为P_I的初始位置误差并在后续所有前向传播中被积分放大。Fast-LIO 原生的 IMU 时间戳对齐是建立在Δt极小的假设上而 GPS 的时间戳却是一个宏观的、带有系统延迟的“事件”。第二重是状态空间的膨胀。原版 Fast-LIO 的状态向量是一个封闭的、自洽的运动学系统。加入 GPS 后我们必须在状态中显式引入W→G的坐标系变换参数如yaw_offset,scale_factor或者将P_I的单位从m改为deg。无论哪种都打破了原有状态向量的物理一致性。R_I描述的是刚体旋转P_I描述的是笛卡尔位移而yaw_offset描述的却是大地测量学中的方位角——它们属于不同的数学范畴却被塞进了同一个x向量里用同一套F_x和Q去传播。这就像把温度计和气压计的数据强行归一化到同一个刻度上。我最终的选择是不修改状态向量而修改观测模型。GPS 不再是一个对P_I的直接观测而是一个对 P_I本文还有配套的精品资源点击获取
