1. 前向推导在整个滤波链路里到底担什么角色第一次翻开 IKFoM 这个库的时候我盯着MTK_BUILD_MANIFOLD那一串括号看了快半小时。它是流形上迭代卡尔曼滤波的一个轻量模板库不绑定任何具体传感器只负责把「状态怎么表示、怎么加、怎么减、协方差怎么传」这几件事抽象干净。现在好几套开源激光惯性里程计方案都把它当滤波内核用所以你只要想碰 LIO 或者 VIO 这类东西前向推导这一段是绕不过去的第一道坎。所谓「前向推导」说白了就是滤波里的 predict 阶段用 IMU 的角速度和加速度把上一时刻的状态和协方差推到当前时刻为后续的量测更新准备一个先验。这件事听起来简单但一旦状态被放到流形上旋转、重力方向这些量不再是一个普通向量加法和减法都得重新定义所有的雅可比也就跟着变了样。我把这部分内容理解成三个层次第一层是数学连续时间下的名义状态方程和误差状态方程第二层是离散化把一个微分方程变成能写进代码的矩阵乘加第三层是工程维度的对齐、符号的方向、时间戳的对齐、协方差的正定性。很多人卡住不是因为数学难而是因为这三层的边界没分清——数学上用左扰动推的公式代码里写成了右扰动或者连续时间推出来的雅可比直接拿去乘 dt 当离散雅可比用小步长下看不出来一到大机动就开始飘。这篇笔记我就按这三层往下拆尽量把每个符号为什么是它讲清楚。适合谁看如果你已经能看懂卡尔曼滤波的五个公式知道协方差传播是P F P F^T Q但一遇到SO(3)、S2这些词就头皮发麻那正好。如果你已经在用这类框架只是把它当黑盒调参那看完前向这部分至少下次发散的时候你知道该去看哪几个矩阵。1.1 从一次 predict 说起一次标准的 predict 调用输入是上一时刻的状态、协方差以及两个 IMU 采样之间的时间间隔输出是当前时刻的状态和协方差。中间做的动作有四个把 IMU 的原始测量减去零偏得到「干净」的角速度和加速度用它们积分出旋转、速度、位置的增量把状态按流形的规则「加上」这个增量用状态转移矩阵把协方差推过去并叠加过程噪声。这四个动作里前两个是运动学后两个是概率论中间的接缝就是流形的 boxplus。我见过不少人写 predict 的时候旋转部分直接用R R * Exp(w*dt)更新协方差那边却拿单位阵糊弄过去理由是「旋转变化不大」。短时间确实看不出问题但 IMU 积分是递推的10ms 一步一分钟就是 6000 步误差累积的斜率完全不一样。前向推导之所以值得单独拿出来学就是因为它是整条链路里唯一一个「每帧都在跑、而且没有任何观测来纠正」的环节它的偏差只能靠后面的量测更新去拉拉不回来就发散。1.2 前向推导必须产出的三样东西我把前向部分的产出归纳成三块缺一不可。第一块是名义状态的预测值也就是不带误差的那个状态本身它决定了下一帧点云或者图像特征该往哪儿投影直接影响配准的初值。第二块是误差状态的转移矩阵 F它描述的是「上一刻的小误差经过这段时间之后变成了多大的误差、跑到哪个分量上去了」这是协方差传播的核心。第三块是过程噪声的离散协方差 Q它描述的是「这段时间里 IMU 的噪声给我们灌进来了多少不确定度」没有它协方差只会因为转移矩阵的性质慢慢塌缩滤波器会变得过度自信。这三块里最容易写错的是 F最容易忽略的是 Q。F 写错通常表现为「滤波器一开始正常跑几十秒之后突然发散或者完全不更新」因为错误的 F 会让协方差的结构变形Q 写错则表现为「静止时协方差一直在涨」或者「动态时滤波器反应迟钝」。后面我会把这两块的推导和常见坑分开讲。1.3 状态量的切分原则状态怎么切直接决定了矩阵的维度和你写代码时的索引。一个比较通用的惯性状态包含位置、旋转、速度、陀螺零偏、加速度计零偏再加上重力方向这个单位向量。再往外如果是激光惯性系统可能还要带上雷达和 IMU 之间的外参。这里有个原则我一直坚持能放到流形上的量绝不放到欧氏空间里。旋转放到SO(3)上是因为旋转矩阵有正交约束你用 9 个数去表示 3 个自由度协方差矩阵必然奇异滤波会崩。重力方向放到S2上是因为它是个单位向量只有两个自由度用三个数表示同样会产生秩亏。剩下的位置、速度、零偏都是老老实实的欧氏向量直接按三维处理就行。这条原则听起来像常识但我确实见过把重力当三维向量塞进状态里的实现跑是能跑但协方差矩阵的第 21、22、23 行永远是相关的数值上一直在跟奇异做斗争。维度算清楚之后索引就固定了。我习惯在代码里把每个块的起止下标写成常量或者枚举别在矩阵填充的时候靠脑算block3,3(12,3)这种偏移那玩意儿改一次状态定义就要全局重来出一次错就是一整天。2. 流形抽象boxplus 与 boxminus 决定了后面所有符号IKFoM 最核心的设计就是那两个运算符boxplus和boxminus。你可以把它们理解成「流形上的加法」和「流形上的减法」。boxplus(x, delta)的意思是在流形元素 x 的切空间里走 delta 这么远落到流形上的哪个点。boxminus(x, y)反过来从 y 到 x在 x 的切空间下的坐标是多少。这两个运算符一套整个滤波框架就可以用统一的写法处理所有状态不用为旋转单独写一套公式、为向量再写一套。这件事的价值在于模板化。你用宏把状态结构体定义出来框架自动帮你拼出整体的 boxplus每个成员各自调自己的 boxplus欧氏向量就是普通加法旋转就是右乘指数映射重力方向就是切空间里走两步再归一化。整体的维度也是各个成员维度之和而不是各自「存储维度」之和。旋转存 9 个数但只算 3 维重力存 3 个数但只算 2 维这个差异由流形自己声明框架去汇总。2.1 误差状态的定义方向这是我踩得最深的一个坑。误差状态有两种定义方式x_true x ⊞ δx真值等于名义值加扰动和x_true δx ⊞ x扰动加在左边。前者叫右扰动后者叫左扰动。两种定义推出来的雅可比不一样符号也不一样混用必炸。IKFoM 用的是右扰动那一套也就是SO3的boxplus(r, v) r * Exp(v)旋转增量在右边乘上去。为什么选右扰动我理解主要是两个原因一是右扰动的误差状态方程在形式上更简洁角速度的雅可比不需要额外乘旋转矩阵二是和 IMU 的物理意义对得上——陀螺仪测的是机体系的角速度增量自然是右乘的。你如果按左扰动去推公式也能推出来但往代码里填的时候就得处处小心一不小心就把R和R^T搞反。提示在你写任何一行雅可比之前先在纸上写下你的 boxplus 定义。这一行字决定了后面所有符号改起来是连锁反应。boxminus也要一致。SO3的boxminus(a, b) Log(b^T * a)注意顺序是从 b 到 a。这个顺序如果反了量测更新里的残差就是负的表现出来就是滤波器越更新越偏。2.2 旋转与重力方向两种典型流形SO(3)这部分大家相对熟指数映射用罗德里格斯公式Exp(θ) I sin|θ|/|θ| [θ]× (1-cos|θ|)/|θ|² [θ]ײ小角度时退化成一阶近似I [θ]×。有个细节|θ|接近 0 的时候除法会出数值问题所以实现里一般会做阈值判断小于 1e-8 就直接用一阶近似。我看过一些自己写的实现没做这个判断静止的时候状态就开始抖原因是加计噪声让角速度在 0 附近来回跳指数映射的除法每次都在放大舍入误差。S2这个流形值得单独说说。重力方向在状态里就是一个单位向量它只有两个自由度切空间是垂直于它本身的那个平面。boxplus(g, δ)的实现思路是先在切平面里构造一组正交基B2×3 或者 3×2取决于你的约定然后g_new g*cos|δ| (B*δ/|δ|)*sin|δ|归一化之后落到球面上。boxminus反过来把两个单位向量的差投影到切平面上δ B^T * (g_target / (g^T g_target))再取对数那一套。说白了就是「在球面上走最短路径」。为什么重力要放进状态里估计因为在没有其他绝对观测的情况下IMU 的俯仰和横滚是可以被重力观测到的把它当状态估计滤波器能自动修正初始姿态的倾角误差。但代价是协方差矩阵多了一个 2 维的块前向传播的时候这一块和速度、零偏是耦合的。2.3 宏展开之后的结构体长什么样用宏定义状态的时候你写的是类似这样的东西MTK_BUILD_MANIFOLD(state_ikfom, ((vect3, pos)) ((SO3, rot)) ((S2, grav)) ((vect3, vel)) ((vect3, bg)) ((vect3, ba)) );宏展开之后会生成一个普通的结构体每个成员就是你写的类型同时生成manifold_dimension()这个静态函数返回 332333 17。还会生成整体的boxplus、boxminus、operator、operator-。你拿到这个结构体之后所有对状态的操作都通过boxplus走不要自己去改成员否则协方差那边的维度就对不上了。有个小技巧如果你的状态里带了外参外参里的旋转也是SO3一样参与 boxplus维度会从 17 涨到 23。这时候所有手写的索引常量全部要变所以我在项目里一律用constexpr算偏移写成IDX_VEL IDX_ROT 3这种累加形式改结构的时候只要改声明偏移自动跟着走。3. 连续时间方程与误差状态推导这一节是整篇笔记里最「数学」的部分但我尽量用工程视角讲重点放在「每一项对应代码里的哪一块」。先把符号约定死R是机体系到世界系的旋转p是位置v是速度ω_m是陀螺仪读数a_m是加计读数b_g、b_a是对应的零偏g是世界系下的重力向量模长固定方向可估。噪声一律用n表示假设是零均值高斯白噪声。3.1 名义状态的运动学名义状态是不带噪声的运动学积分四个方程位置导数等于速度速度导数等于「旋转把机体系加速度转到世界系再加上重力」旋转导数等于「旋转矩阵乘以角速度的反对称阵」零偏导数为零假设零偏是常值变化用随机游走建模放在噪声里。这四个方程写出来是三行代码的事但里面有两个细节值得抠。第一个细节是重力项的符号。世界系下重力指向下方如果你的世界系是 z 轴朝上重力向量就是[0, 0, -9.81]。很多实现里把grav定义成「重力方向」这个单位向量实际用的时候乘上模长G再取负号。这个负号如果搞反静止的时候速度会一路往下冲滤波器会疯掉。我的习惯是在初始化的时候打印一下grav和实际的加计读数静止时加计读数应该是-g因为加计测的是比力不是重力加速度本身对不上就说明符号错了。第二个细节是零偏的建模。名义状态里零偏是常量但真实的零偏会缓慢漂移这个漂移用随机游走噪声n_bg、n_ba描述在误差状态方程里体现为δḃ n_b。零偏噪声的谱密度一般查 IMU 手册查不到就用「静止两小时零偏的方差随时间线性增长」这个性质去反推斜率的平方除以 2 差不多就是谱密度。3.2 误差状态方程逐项推导误差状态的推导用的是「真值 名义值 ⊞ 误差」这个关系代入真值的运动学方程减去名义值的方程丢掉二阶小量。我按分量拆开讲重点是每个雅可比是怎么冒出来的。位置最简单δṗ δv。没有任何旋转参与纯线性。速度稍微复杂。真值的速度导数等于「真值旋转乘上真值加速度减零偏、减噪声加真值重力」。把真值旋转R(I [δθ]×)展开加速度里的零偏和噪声写成â - δb_a - n_a乘开之后会多出一项R[δθ]×â而[δθ]×â -[â]×δθ所以这一项变成-R[â]×δθ。这就是速度误差方程里那个-R[â]×的来源也是整份 F 矩阵里最容易写错符号的地方。重力项同理g_true g ⊞ δg在切空间上是线性的系数就是模长G。旋转是最绕的。真值旋转的导数等于真值旋转乘上真值角速度的反对称阵。把R(I [δθ]×)代进去、两边展开、利用[a]×[b]× - [b]×[a]× [a×b]×这个恒等式最后能得到δθ̇ -[ω̂]× δθ - δb_g - n_g。这里的ω̂ ω_m - b_g。注意这个负号它来自右扰动的定义如果你用左扰动推这项会变成正号而且δb_g的系数也会变。零偏δḃ_g -n_bg、δḃ_a -n_ba符号取决于你噪声项的正负约定只要和你的 G 矩阵一致就行。这一块错了不会直接发散但会让过程噪声的实际效果和你想的不一样。重力方向在S2的切空间上它的导数是零世界系下重力方向固定但它和陀螺零偏是耦合的——因为零偏误差会让姿态估计产生偏差姿态偏差又会影响重力方向在切空间里的表示。这个耦合项在 F 矩阵里是一个 2×3 的块具体系数和切空间基的选取有关也是我在代码里反复核对过最多遍的地方。3.3 连续雅可比与噪声映射矩阵把上面每一项整理成矩阵你就得到了连续时间的F17×17和G17×12因为角速度和加速度各 3 轴零偏随机游走各 3 轴一共 12 个噪声源。F 的非零块大致是位置对速度速度对旋转、对加速度零偏、对重力旋转对旋转自身、对陀螺零偏其余是零。G 的块更少速度对加速度噪声-R旋转对角速度噪声-I两个零偏各自对自己的随机游走噪声。这里的经验是先把 F 的稀疏结构画出来再往里填数。我现在习惯在纸上画一个 17×17 的格子把非零块标出来每个块旁边写上「这个块描述什么」填代码的时候照图抄出错率能降一大半。纯对着公式填矩阵第一次几乎不可能全对。4. 离散化落地F、Q 怎么算才不会发散连续方程推完离能跑的代码还差一步离散化。IMU 是离散采样的你的 predict 是每隔 dt 推一次所以要把ẋ Fx Gw变成x_{k1} Φ x_k w_d。这一步的近似方式直接决定了滤波器在大 dt 下的稳定性。4.1 一阶离散化与步长选择最常用的是零阶保持加一阶泰勒Φ ≈ I F·dt。这个近似在|F·dt|的二范数远小于 1 的时候足够准通俗点说就是「这段时间里误差的变化率几乎不变」。IMU 一般 100~200Hzdt 是 5~10ms角速度就算跑到 10 rad/s[ω]×·dt的量级也就 0.1一阶近似完全够用。但如果你的 IMU 只有 50Hz或者你把多帧 IMU 攒起来一起推dt 到了 50ms一阶近似就开始不准了尤其是旋转那部分误差会体现在协方差的迹上——你会看到协方差的迹比理论值偏小滤波器变得过度自信。如果确实需要大 dt可以上二阶Φ ≈ I F·dt (F·dt)²/2代价是矩阵乘法的次数翻倍。我的做法是留一个开关dt 超过 20ms 就自动切二阶实测下来比硬调噪声参数管用得多。还有一个坑别把连续时间的 F 直接乘 dt 当离散 F 用然后 Q 那边又按另一套口径算。这两者必须来自同一个离散化假设否则噪声的实际强度和你的协方差结构对不上。我见过一个实现F 用一阶Q 用G Q_c G^T dt看上去都合理但因为少了一次Φ的左右乘协方差在旋转和速度的耦合块上一直是歪的最后的表现是姿态估计在高动态下有系统性滞后。4.2 噪声协方差离散化的两种口径第一种是最简的Q_d G Q_c G^T · dt。这个式子假设噪声在 dt 内是分段常值的忽略了一阶以上的传播效应在小 dt 下够用实现只有两行。第二种稍严谨Q_d (Φ G Q_c G^T Φ^T G Q_c G^T) · dt / 2。这个式子考虑了噪声在 dt 内也被状态转移「搬运」了一次两者取平均。差别在哪对于速度通道这种和旋转强耦合的分量第二种算出来的 Q 会略大一点滤波器的响应更「活」。实测下来如果你的 dt 小于 10ms、并且 IMU 噪声参数填得比较准两种口径的差异在几分钟的跑合里看不出来。但如果你的噪声参数是拍脑袋填的大部分情况都是第二种口径容错性更好因为它不会让协方差塌得太快。我现在的默认选择是第二种代价是每帧多两次矩阵乘法对现代处理器来说忽略不计。4.3 对称化与数值卫生协方差矩阵理论上永远对称正定但浮点运算跑上几万帧之后它就不对称了再过一阵子最小特征值可能变成负数。这不是理论问题是必然会发生的工程问题。三个动作基本能兜住一每次传播完之后强制对称化P (P P^T) / 2二定期检查最小特征值低于阈值就说明模型和实际差太多该调噪声了三别用P.inverse()用 Cholesky 分解或者 LDLT。还有一个容易被忽略的点状态更新完之后要重新归一化流形上的量。旋转矩阵乘了几万次之后会慢慢不正交重力向量会慢慢不是单位长度。这些偏差很小但会在 boxplus/boxminus 里被放大。我的习惯是每帧更新完对旋转做一次 SVD 正交化或者用四元数做中转对重力向量做一次归一化成本可以忽略稳定性提升很明显。5. 一份可运行的传播代码骨架理论讲完落到代码。我不打算贴某个具体库的源码那玩意儿版本一换就对不上而且直接抄源码对理解没帮助。下面这套骨架是我按常见实现的思路重新组织的维度按 17 维来不带外参你把它填进自己的工程里配合自己定义的SO3、S2类型就能跑。5.1 状态与矩阵维度的对齐先定义索引这一步别偷懒constexpr int DIM 17; constexpr int IDX_POS 0; // 3 constexpr int IDX_ROT 3; // 3 constexpr int IDX_GRAV 6; // 2 constexpr int IDX_VEL 8; // 3 constexpr int IDX_BG 11; // 3 constexpr int IDX_BA 14; // 3注意重力占 2 维不是 3 维。这一行如果写成 3后面所有索引全错而且错得很隐蔽——矩阵能乘结果就是不对查起来能把人逼疯。我建议在初始化的时候加一个静态断言把IDX_BA 3 DIM写死编译期就能拦住。状态结构体按宏的形式定义然后写两个辅助函数state_boxplus(s, delta)和state_boxminus(a, b)内部逐成员调用各自的流形运算。这两个函数是后面所有代码的基础写完先用几个手算的例子验一遍比如旋转加一个绕 z 轴 90 度的增量看看结果对不对。5.2 predict 函数主体逻辑大概是这样void predict(State s, MatDIM,DIM P, const ImuMeas m, double dt, const Mat12,12 Qc) { // 1. 去零偏 Vec3 w m.gyro - s.bg; Vec3 a m.acc - s.ba; // 2. 名义状态积分用中点或 RK2 都行一阶欧拉最省 Vec3 acc_w s.rot * a GRAVITY * s.grav; // 世界系加速度 s.pos s.vel * dt 0.5 * acc_w * dt * dt; s.vel acc_w * dt; s.rot s.rot * SO3::Exp(w * dt); // 右扰动右乘 // 3. 构造连续雅可比 F只填非零块 MatDIM,DIM F MatDIM,DIM::Identity(); F.block3,3(IDX_POS, IDX_VEL) Mat3::Identity(); F.block3,3(IDX_VEL, IDX_ROT) -s.rot * skew(a); F.block3,3(IDX_VEL, IDX_BA) -s.rot; F.block3,2(IDX_VEL, IDX_GRAV) GRAVITY * dgrav_ddelta(s.grav); F.block3,3(IDX_ROT, IDX_ROT) -skew(w); F.block3,3(IDX_ROT, IDX_BG) -Mat3::Identity(); // 4. 离散化 MatDIM,DIM Phi MatDIM,DIM::Identity() F * dt; MatDIM,12 G MatDIM,12::Zero(); G.block3,3(IDX_VEL, 0) -s.rot; G.block3,3(IDX_ROT, 3) -Mat3::Identity(); G.block3,3(IDX_BG, 6) Mat3::Identity(); G.block3,3(IDX_BA, 9) Mat3::Identity(); MatDIM,DIM Qd (Phi * G * Qc * G.transpose() * Phi.transpose() G * Qc * G.transpose()) * (0.5 * dt); // 5. 协方差传播 对称化 P Phi * P * Phi.transpose() Qd; P 0.5 * (P P.transpose()); }几处值得说明。第一速度的积分我用了0.5*a*dt²这一项对位置的影响在小 dt 下确实很小但它对「静止时位置漂移」这个现象有抑制作用加上不亏。第二F里旋转对自身的块是-skew(w)这是右扰动下的结果如果你用的是左扰动这里应该是-skew(w)变成别的形式别照抄。第三dgrav_ddelta是那个把 3 维方向映射到 2 维切空间的矩阵自己实现的时候要把切空间基的构造写对这地方我返工过三次。5.3 有限差分自检写完别急着上真实数据先做一个有限差分自检随机生成一个状态和一个小扰动用boxplus得到一个「真值」然后用你的运动学方程分别对真值和名义值积分一小步两者的差应该约等于F·δ·dt。具体的验证方式是对每个状态分量取δ e_i * εε 取 1e-6 左右算出(f(x ⊞ δ) ⊟ f(x)) / ε和你的解析雅可比逐元素对比相对误差大于 1e-4 就说明那一列写错了。def check_jacobian(f, boxplus, boxminus, x, eps1e-6): n len(x) J_num np.zeros((n, n)) f0 f(x) for i in range(n): d np.zeros(n); d[i] eps J_num[:, i] boxminus(f(boxplus(x, d)), f0) / eps return J_num # 和解析 F 对比这个自检我建议做成常驻的单元测试每次改状态定义或者改雅可比都跑一遍。它救过我至少两次——有一次我把速度对零偏的符号写反了肉眼看公式完全看不出来有限差分一跑那一列全是反的一眼就定位了。6. 排查实录与速查表真正跑起来之后遇到问题的概率远高于纸面推导出错因为纸面错了你至少知道大概在哪工程问题往往表现为「滤波器就是不对但不知道哪里不对」。下面几个是我印象最深的。6.1 协方差的迹只涨不落症状是跑起来之后trace(P)一直线性增长量测更新也压不下去。第一反应通常是过程噪声 Q 太大把它调小之后好了一点但治标不治本。真正的原因大概率是量测更新那边用的误差状态定义和前向传播不一致。前向推的是右扰动量测雅可比按左扰动写的两边的协方差描述的不是同一个误差更新的时候自然对不上。判断方法很简单把 Q 设成零静止状态下跑如果trace(P)还在涨那肯定是 F 或 G 填错了如果不动了接上 Q 再涨那就是噪声参数的问题。这个方法我称之为「分离变量法」能快速把问题框到某一个矩阵上。6.2 静止时姿态缓慢旋转静止的时候滤波器应该是收敛的如果你看到姿态估计在缓慢漂移或者重力方向的估计一直往某个方向偏八成是重力方向和加速度计零偏的可观测性冲突。这两个量在没有旋转激励的情况下是耦合的重力方向偏一点、加速度零偏偏一点可以让残差都是零滤波器分不清到底是谁的问题。解法有两个方向一是把加速度零偏从状态里拿掉当成常值标定好二是让初始化阶段多静止一会儿并且给零偏一个比较强的先验初始协方差给小。我自己更倾向第一个因为廉价的 IMU 零偏本身漂得就慢估它带来的麻烦比收益多。6.3 时间戳对不齐导致的抖动这个坑很隐蔽。如果你拿到的 IMU 时间戳和实际采样时刻差了几毫秒每帧的 dt 就会在标称值附近抖动积分出来的速度和位置会带上一个高频抖动看起来像噪声实际是时间同步问题。判断方法是把 dt 打印出来看它是否恒定如果有周期性波动往时间戳对齐的方向查。6.4 常见症状速查表症状最可能的原因优先检查位置协方差爆发式增长旋转雅可比符号反了F的(ROT, ROT)块静止时速度持续劣化重力项符号错速度方程里的GRAVITY * grav姿态估计有系统偏置右扰动与左扰动混用boxplus定义与量测雅可比协方差矩阵非正定未对称化 / 累积误差每帧结束后加对称化大机动下突然发散dt 过大 一阶离散切二阶离散或提高 IMU 频率位置缓慢漂移加计零偏与重力可观测性冲突状态定义里是否含ba高动态下响应滞后Q 给小了 / 一阶近似不够Qc参数与离散口径再补一条我个人的经验遇到问题先打印协方差的块对角别打印整个矩阵。17×17 打出来根本看不清把旋转块、速度块、重力块各自的对角线拉出来看哪一块的数值量级不对问题的位置基本就锁定了。这个方法比调参数管用一百倍。7. 我在实际项目中攒下的几条体会前向推导这部分纸面上推一遍大概两小时把它写对并且调稳我花了差不多两周。回头看我自己的学习路径有几个点如果早知道能省很多时间。第一先写数值自检再写实现。很多人包括我习惯先把公式敲成代码跑通了再想着验证但「跑通」和「跑对」是两回事。正确的顺序应该是先把有限差分的框架搭起来然后每写一个雅可比块就跑一次对比错了立刻定位到那一块。这样虽然前期慢但总时间反而短。第二把符号约定写在代码注释的第一行。右扰动、世界系重力向下、噪声项取负号这三条写清楚半年之后你自己回来看还能接得上。我有个项目因为换了台电脑、注释没带过去回头 debug 的时候花了整整一天才把符号体系重新捋顺。第三别怕把 dt 切小。如果你的 IMU 支持 200Hz 以上就用 200Hz别为了省算力把多帧 IMU 攒成一个大 dt 去推。前向传播的计算量本来就是整个滤波里最小的部分省这点算力换来的是稳定性下降完全不划算。真正吃算力的是量测更新和点云配准。第四重力方向在S2上的估计初期别放太松。给一个大一点的过程噪声能加快收敛但收敛之后如果噪声还很大重力方向的估计会跟着姿态一起抖。我的做法是分层前 5 秒用大噪声快速收敛之后切到小噪声稳住。这个切换逻辑很简单但效果比一路用固定参数好太多。最后说个后续可以扩展的方向。这个骨架搭好之后往里面加外参雷达和 IMU 之间的旋转平移是很自然的只要在外参的旋转上也用SO3、平移用欧氏向量维度扩到 23 就行。再往上如果你的系统有多传感器也可以把每个传感器的时延当状态估进去——本质上还是同一套 boxplus 的玩法把「时延」当成一维欧氏量加进状态即可。前向推导这部分吃透了后面的东西都是在这个骨架上挂肉。