把 IKFoM 的前向推导啃下来，我前后断断续续花了差不多三周，中间推翻重写过两次。第一次以为自己看懂了，结果一动手写模型就发现雅可比对不上，状态一下就跑飞；第二次把维度当成纯数字硬背，换了台设备、换了个外参，整条链路直接不收敛。真正让我开窍的，是意识到它其实只干了一件很朴素的事：把流形上的运动模型和观测模型，按“切空间”这套语言重新组织前向传播，然后让框架去接管那些重复的链条工作。所谓前向推导，说白了就是从输入和上一时刻状态出发，一路算出预测状态、预测协方差、残差和观测雅可比的全过程；反向的部分（迭代更新、增益求解）交给框架。很多人卡在前向，其实卡的不是公式，而是三件事没对齐：流形的加减法到底怎么定义、误差状态在哪个切空间里、以及噪声雅可比和 Q 矩阵的顺序有没有对上。这篇笔记就围绕这三件事展开，写成一篇可以照着复现的操作记录，不论你是刚接触流形滤波器的新手，还是已经在调参但总觉得哪里不对的老手，都能从里面找到能直接用的东西。

1. 前向推导在 IKFoM 里到底指什么

1.1 先把“前向”这个词的边界划清楚

我最初看 IKFoM 的代码，最大的困惑是：它明明叫“迭代卡尔曼滤波”，为什么学习资料总是把“前向推导”单独拎出来讲？后来才明白，这里的“前向”指的不是神经网络的前向传播，而是滤波流程里从上一时刻到预测、再从预测量到观测量这一段纯计算链路。它不涉及最优增益的求解，不涉及迭代收敛判断，也不需要求逆。你只要保证这一段算得对，后面的更新步基本是框架自动完成的；这一段算错，后面再精细的迭代也救不回来。

具体来说，前向推导要产出四样东西。第一是预测状态，也就是把 IMU 读数积分一段时间后，载体应该到了哪里、姿态转成了什么样。第二是预测协方差，描述这份预测有多不确定，它是靠误差状态的递推关系一路传下来的。第三是残差向量，把预测状态映射到观测空间，和真实观测做差。第四是观测雅可比，也就是残差对误差状态的偏导，它决定了更新时该往哪个方向修正。这四样东西构成了整个滤波器的骨架，反解增益不过是在这个骨架上做的一次线性代数运算。

理解了这一点，你就知道前向推导的“推导”二字指的是什么：它不是让你从头证明一个定理，而是让你在流形的切空间里，把误差状态的递推关系和观测关系的偏导一条条写出来。写对了，滤波就稳；写漏一项，误差就会被积分放大。这也是为什么 IKFoM 的学习曲线陡峭——它要求你必须先把物理模型和数学表达对齐，代码只是最后的落地。

1.2 为什么不能直接照抄欧氏空间的 EKF 公式

刚上手的时候，我做过一个很典型的错误尝试：把姿态当成一个四元数，直接套欧氏空间的 EKF 公式，把四元数四个分量和位置、速度拼成一个 16 维向量，然后照抄F = I + A·dt、P = F P F^T + W Q W^T。跑起来前几帧看着还行，运动一复杂就立刻发散了。

问题出在哪儿？四元数有一个硬约束：模长必须为 1。如果你把它当成欧氏空间里的四个自由变量去做加法，q + δq的结果几乎必然不满足模长为一，于是每一帧都要归一化，而归一化本身是一个非线性操作，它会把你在协方差里积累的信息一点点吃掉。更麻烦的是，协方差矩阵会沿着“模长方向”不断膨胀，而这个方向在物理上根本不存在。这就是所谓的过参数化问题：状态用了四个数表达，但真实的自由度只有三个。

IKFoM 的解法是把加减法重新定义。它不去直接对四元数的四个分量做运算，而是引入一个三维的扰动量，通过一个专门的算子把它“加”到四元数上——先在切空间里做欧氏运算，再映射回流形。这样一来，协方差只在三维切空间上传播，模长方向被彻底隔离掉了。这个重定义就是boxplus和boxminus两个函数，是整篇推导的地基。我建议你在看任何公式之前，先把这两个函数的实现读三遍，读懂了再往下走，否则后面所有的雅可比你都会觉得“凭空冒出来”。

同样的逻辑适用于其他流形。方向向量（比如重力方向）只有两个自由度，如果你用三维向量去表达，就会多出一维；旋转矩阵有九个元素但只有三个自由度，直接当九维用更是灾难。IKFoM 通过模板把这些流形统一抽象出来，让每个流形自己声明自己有多少自由度、怎么加、怎么减，框架层面只认“自由度”这一件事。

1.3 IKFoM 的三层抽象：从 MTK 到 esekf

把代码翻一遍，你会发现整个框架其实分三层，理解分层能省下大量读代码的时间。

最底层是流形工具箱，负责定义各种流形类型的基本运算。它提供的是编译期的东西：每个流形要声明自己的自由度、要给出boxplus/boxminus的具体实现、要提供从切空间到流形和从流形到切空间的映射。这一层的价值在于，你可以自己定义新的流形，只要满足接口约定，框架就能用。

中间层是状态组装层，也就是用宏把多个流形拼成一个状态结构体。这一层做的是机械工作：根据每个成员的自由度，累加出整个状态的自由度；生成遍历式的构造函数；生成整个状态级别的boxplus/boxminus，内部按成员逐个调用各自的实现。你在业务代码里写的那个状态结构体，就是这一层产出的。

最上层是滤波器本体，它接收四个用户函数：状态转移函数、状态转移对状态的雅可比、状态转移对噪声的雅可比、观测函数。滤波器本身不关心你的状态里有什么，它只按自由度做统一的协方差传播和迭代更新。这一层的设计非常克制，它把所有领域知识都推给了用户函数，自己只管流程调度。

三层看清楚了，你就会明白前向推导的工作量到底落在哪：底层和中间层是一次性配置，真正需要你反复推敲的是最上层那四个函数。接下来的章节就是围绕这四类程序展开的。

2. 状态流形建模：把二十多维塞进一个结构体

2.1 宏展开后到底生成了什么

状态组装用的是宏，写法非常简短，几行就搞定了。但宏的展开结果值得认真看一眼，因为它决定了后面你能调用哪些接口。

以一个典型的激光惯性系统为例，状态里通常包含位置、姿态、外参旋转、外参平移、速度、陀螺零偏、加计零偏、重力方向。用宏写出来大概是这个形态：

MTK_BUILD_MANIFOLD(state_ikfom, ((vect3, pos)) ((SO3, rot)) ((SO3, offset_R)) ((vect3, offset_T)) ((vect3, vel)) ((vect3, bg)) ((vect3, ba)) ((S2, grav)) );

这段宏展开之后，至少会生成四类东西。第一是成员变量本身，名字就是你在括号里写的名字，类型是流形类型。第二是一个编译期常量，记录整个状态的切空间自由度总和；注意这个值不等于成员变量占用的浮点数个数，因为SO3内部往往是四个数但只算三个自由度，S2内部三个数只算两个自由度。第三是状态级别的加法减法，实现方式是按成员逐个调用，再把各自的切空间分量拼成一个大向量。第四是一些辅助接口，比如按索引取值的访问器、判断是否有限的检查等。

我第一次用的时候犯过一个很低级的错误：我以为宏里写的顺序无所谓，结果在手动拼雅可比的时候，行块顺序和宏里写的顺序不一致，导致整个 F 矩阵错位。这种错误特别隐蔽，因为矩阵维度是对的，编译能过，跑起来只是“不太准”。所以我的建议是，宏里成员的顺序定下来之后就别再动了，并且在雅可比函数里用注释把行块顺序标出来，跟宏里的顺序严格对应。

2.2 boxplus 与 boxminus：流形上的加减法到底在做什么

这两个函数是整套推导的核心接口，我单独拎出来讲。

boxplus的语义是：给我一个流形上的元素，再给我一个切空间里的扰动向量，返回扰动后的流形元素。数学上写作X ⊞ δ。boxminus反过来：给我两个流形元素，返回它们之间的切空间差，写作X1 ⊟ X2。这两个算子必须满足一致性：(X ⊞ δ) ⊟ X ≈ δ，在 δ 足够小的时候成立。

对于旋转这种流形，常见实现是这样的思路：把三维扰动向量除以二，拼成一个标量部分为一、向量部分为扰动一半的四元数，归一化之后右乘到原四元数上。为什么除以二？因为四元数表示旋转时存在一个二对一的映射，旋转一个微小角度对应的四元数增量，其向量部分大约是角度的一半。这个近似在小角度下非常精确，而且避免了三角函数调用，在嵌入式平台上很划算。

对于方向向量这种流形，做法是先在当前向量的切平面里构造一组正交基，然后用两个扰动分量分别沿这两个基方向偏移，最后重新归一化。这里有个坑我必须提醒：当扰动量比较大的时候，直接线性叠加再归一化会引入明显的幅值误差，有些实现会用正弦余弦去做精确映射。如果你的系统里方向向量的扰动经常超过零点几弧度，就要确认一下手里的实现是哪一种版本。

提示：自己实现流形时，boxplus和boxminus必须严格互逆。我见过有实现里boxplus用了右乘、boxminus用了左乘，单独测都能过，一进滤波器就发散。测试方法很简单：随机取若干流形元素和随机扰动，验证(X ⊞ δ) ⊟ X与δ的差在机器精度量级。

2.3 维度对账：状态维、切空间维、协方差维

这是最容易出错的地方，我专门做了一张对账表。整篇推导里出现的所有矩阵维度，都必须能在这张表里找到出处。

表里最反直觉的是“流形存储维”和“切空间维”不相等。以上面的状态为例，两个旋转各多存一个数、方向向量多存一个数，加起来存储维比切空间维多三个。协方差矩阵是切空间维的方阵，所以它比“把结构体拍平”得到的维度要小。如果你按存储维去开协方差矩阵，编译期可能不报错（因为模板参数是你自己填的），运行起来就是内存越界或者静默错位。

我在一个项目里就吃过这个亏：手动写了一个 26 维的协方差，滤波器跑起来数值看着正常，但定位曲线在高动态段会有规律地抖动。查了两天才发现是维度多开了三个，多出来的那几行几列一直是零，导致协方差在迭代更新时求逆不稳定。改成正确的自由度之后，抖动立刻消失。

3. 预测步前向推导：从力学方程到 F 矩阵

3.1 连续时间运动学：把物理写清楚

预测步的物理基础是惯导方程，这部分是教科书内容，但落到代码里有一个细节必须注意：状态里到底包含了哪些量，直接决定了方程要写几项。

标准的一组方程是这样的：位置对时间的导数是速度；速度对时间的导数是“旋转后的比力减去零偏，再叠加一个重力项”；姿态对时间的导数是“角速度减去陀螺零偏”对应的旋转增量；两个零偏的导数是随机游走，也就是噪声；重力方向在短时间内视为常量，导数为零。

写代码的时候，很多实现会把重力方向当成状态量一起估计，这样可以抵消一部分初始对准的误差。这时候要特别注意：重力方向作为一个方向向量参与运算时，它必须是单位向量，姿态旋转后的比力减去重力，这两项的坐标系要统一。我见过有实现在这一步混用了世界系和机体系的量，结果重力项符号反了，静止时速度会缓缓漂移，看起来像是零偏没标定好，实际上是坐标系错了。

另一个容易忽略的点是，零偏在状态里是“待估计的量”，而在方程里用它做补偿时，要用的是当前估计值。这两者一旦混淆，你就会在误差递推里多写一项。记住一个原则：标称状态用估计值推进，误差状态围绕标称状态线性化，两者不要互相污染。

3.2 中值积分：为什么不用简单的欧拉法

离散化这一步，很多教程一笔带过，说“用一阶欧拉就行”。实际做过的人都知道，纯一阶欧拉在采样率不高或者角速度大的时候误差很明显。

原因很直观。一阶欧拉假设在这一小段时间内，旋转后的加速度是恒定不变的，于是位置增量直接用“当前速度乘时间”加上“当前加速度乘时间平方的一半”。但载体在这段时间内其实已经转了，旋转后的加速度方向早就变了。如果角速度比较大，这个方向偏差会被时间平方放大，表现就是高速转弯时轨迹往外扩。

中值积分的思路很朴素：取这一小段时间首尾两个时刻的角速度和加速度，用它们的平均去更新姿态，再用更新后的姿态去算加速度的积分，最后用首尾两个速度的平均去更新位置。多出来的计算量很小，但在动态场景下精度提升非常明显。我在一个手持设备的数据上对比过，同样的采样率，纯欧拉在快速甩动时位置误差能到分米级，中值积分能压到厘米级以内。

实现上有两个坑。第一个是首尾时刻的零偏要用同一个值，如果你在两端各用了一次当前估计的零偏，而中间滤波器更新过，就会出现一段莫名其妙的速度跳变。第二个是首尾两个姿态最好都做一次归一化，不要等到最后统一处理。

3.3 误差状态递推：一块一块推 F

这是整个前向推导里最需要耐心的部分。方法上没什么花哨的，就是围绕每个误差状态分量，问一句“它的一阶导数里，包含哪些其他误差状态分量的线性项”，然后把系数填到对应位置。

我习惯先列出误差状态的排列顺序，明确写下“第 0 到 2 是位置误差，第 3 到 5 是速度误差，第 6 到 8 是姿态误差”这样的注释，然后逐块填。

位置误差对时间的导数就是速度误差，所以 F 里对应的那块是单位阵。速度误差的导数里，包含姿态误差通过比力反对称矩阵耦合进来的项、包含加计零偏误差的项、包含重力方向误差的项。这三项系数分别是“旋转矩阵乘比力反对称阵取负”、“旋转矩阵取负”、“单位阵取负”。姿态误差的导数里，包含它自己和角速度反对称阵的乘积，以及陀螺零偏误差。

填完之后用F = I + A·dt做一阶离散化。这里要注意，A 是连续时间的系数矩阵，dt 是积分步长。有的实现会用更高阶的离散化，比如把 A 的平方项也带上，但在几十到几百赫兹的采样率下，一阶已经够用，除非你的系统采样率特别低。

我踩过的一个坑是：反对称矩阵的符号。同一个物理耦合，写成[a]×还是-[a]×，完全取决于你定义误差扰动时是“左扰动”还是“右扰动”。这两种约定在文献里都常见，混用就会出现“转弯时速度估反”的问题。我的做法是，先固定一种约定，然后在代码注释里把约定写清楚，之后所有的雅可比都按这个约定推。这样即使以后回头看，也不至于重新推一遍。

3.4 噪声雅可比与协方差传播

噪声雅可比 W 的形状是“切空间维 × 噪声维”。填写规则很简单：哪个误差状态的导数里直接出现了哪一路噪声，就在对应位置填系数，其余位置为零。

典型的四路噪声是：陀螺白噪声、加计白噪声、陀螺零偏游走噪声、加计零偏游走噪声，各占三维，共十二维。姿态误差的导数里直接有陀螺白噪声，填负单位阵；速度误差的导数里直接有加计白噪声，填负单位阵；两个零偏的导数里各自有对应的游走噪声，填正单位阵。

注意：噪声向量的排列顺序必须和协方差矩阵 Q 的顺序严格一致。我见过一个实现里，噪声雅可比按“陀螺、加计、陀螺零偏、加计零偏”排，而 Q 矩阵按“陀螺零偏、加计零偏、陀螺、加计”填，两边都没错，但配在一起就完全错了。表现是零偏估计不但不收敛，还会随时间缓慢漂移。排查这种问题，最有效的办法是把 Q 写成对角阵，每个对角元素给不同的量级，然后单轴激励看响应。

协方差传播就是标准的P = F P F^T + W Q W^T。这里有两个数值上的处理值得加进去。第一，乘完之后强制对称化，也就是取(P + P^T) / 2，因为浮点误差会让 P 慢慢失去对称性，而后续求逆对不对称很敏感。第二，可以给对角线加一个极小的正数，防止某些分量长期零激励导致 P 变得接近奇异。这两个处理加不加，在短时间测试里看不出区别，但跑几个小时之后差别就出来了。

4. 观测步前向推导：残差、雅可比与迭代

4.1 残差函数的书写规范

观测函数要返回两样东西：残差和观测雅可比。残差的写法看着自由，其实有几个约定必须遵守。

首先是残差的方向。常见约定是“观测值减预测值”，也有反过来的。这个方向决定了雅可比的符号，也决定了后续更新时是加还是减。你只要在整个系统里保持一致就行，但如果混用了两个来源的观测（比如一个模块用前者、另一个用后者），就会出现“一半观测在拉、一半在推”的怪现象。我的做法是统一成“预测减观测”，然后在雅可比里对应取负，这样至少符号逻辑是统一的。

其次是残差的归一化。对于距离类的观测，残差直接是米；对于角度类的观测，残差是弧度。两者量级不同，如果不做归一化，最小二乘解会被大量级的观测主导。正确做法是在构造观测雅可比时，把每一维的噪声标准差除进去，等价于对残差做加权。这件事很多人是在 Q 或者 R 里做的，但如果你在雅可比里就除掉了，计算会更干净。

第三是残差的维度必须固定。有些实现里，某一帧观测到的点数是可变的，于是残差向量的长度也变。这在迭代滤波里会出问题，因为协方差更新需要固定的观测维度。常见做法是在一帧内只处理固定数量的观测，或者把多个观测拆成多次更新调用。我倾向于后者，虽然调用次数多，但每次的维度都是编译期常量，矩阵运算可以用固定尺寸，速度快很多。

4.2 以点到面为例拆解观测雅可比

点到面是最经典的观测模型，也是最好拿来练手的。残差的构造是：把机体系下的一个点，经过外参变换到世界系，然后计算它到某个平面的有符号距离。平面用“世界系下的一个点和法向量”表示，距离就是“点减平面上一点，再点乘法向量”。

对位置误差求偏导，结果是法向量的转置。这一项很直观：位置往法向量方向挪一点，距离就变一点。

对外参平移求偏导，结果同样是法向量的转置。因为外参平移直接叠加在点上，和位置是同一个作用路径。这一项很多人会漏，尤其是当外参被当成常量时。但如果你的外参在状态里，就必须写。

对姿态误差求偏导，是链条最长的一项。路径是：姿态扰动影响旋转矩阵，旋转矩阵作用在机体系点上，得到世界系下的偏移量，再点乘法向量。展开之后，系数是“法向量转置乘旋转矩阵乘点的反对称阵，再取负”。这里的负号来自扰动定义，你要根据自己的左右扰动约定确认一次。

对外参旋转求偏导，形式和对姿态的那一项完全一样，只是把外参旋转和外参平移换进去。因为两者在数学结构上是同构的。

我把这一项写在纸上推了三遍才对上符号。第一遍错在把反对称阵写在了旋转矩阵左边，第二遍错在漏了负号，第三遍才和数值验证对上。强烈建议你写完之后，用数值微分做一次交叉验证：给状态加一个极小扰动，看残差变化量和解析雅可比乘以扰动的结果是否一致，误差应该在扰动量的高阶。这个验证花不了十分钟，能省下几天的调试时间。

4.3 迭代更新的收敛判据与提前退出

IKFoM 的“迭代”体现在更新步会重复若干次：每次在当前线性化点重新计算残差和雅可比，求一次增量，更新状态，然后判断是否收敛。前向推导在这里的职责是，保证每次迭代重新调用观测函数时，输入的是最新的状态。

这里有个隐蔽的陷阱。因为是迭代，观测函数会被调用多次，如果你的函数里有“只应该执行一次”的副作用，比如往某个全局容器里 push 数据、或者累加计数器，就会出问题。我见过一个实现里，观测函数内部对点云做了滤波，每次迭代都滤一次，迭代五次之后点云被滤得只剩几个点，收敛判据直接失效。正确的写法是观测函数纯函数化，只读不写，所有预处理放在外面做完。

收敛判据一般是看这次迭代的增量范数是否小于阈值，或者看残差的变化量是否足够小。阈值怎么定？我的经验是取状态量纲的千分之一到万分之一之间。位置单位是米，阈值取 1e-3 到 1e-4 米比较合适；姿态用弧度，取 1e-4 到 1e-5。阈值太大，迭代一两次就退出，等于没迭代；阈值太小，迭代次数打满，实时性崩掉。

还有一点值得一提：迭代次数上限不要设太大。实测下来，绝大多数帧在两到三次迭代内就收敛了，设到十次只是为了兜底。如果你的系统经常打满迭代次数，那说明模型有问题，不是迭代次数不够。

5. 排错实录与参数调试清单

5.1 五个典型现象与排查路径

下面这张表是我在实际项目里积累的，按“现象—可能原因—验证方法”组织，遇到问题可以直接对照着查。

第一条我要特别展开。静止漂移是最容易被误判成“零偏没标定好”的问题。判断方法很简单：把载体完全静止放在水平面上，采集一分钟，如果速度估计单调朝一个方向走，那就是重力项的问题；如果速度在零附近上下抖，那才是噪声问题。前者需要改公式，后者调参数就行。

第四条也值得展开。P 矩阵在长时间运行后失去正定性，是一个非常经典的数值问题。除了前面说的对称化和加小量，还有一个办法是在每次更新后做一次特征值截断，把小于零的特征值强制置为一个小正数。这个操作代价不低，不建议每帧都做，可以每隔若干帧做一次，或者只在检测到异常时做。

5.2 一份可以直接抄的参数起点

参数没有普适值，但有一个合理的起点能省掉大量试错。下面这份表是我在几个不同平台上收敛比较快的初始配置，你可以从这里出发再微调。

调参的顺序建议是：先调观测噪声，再调过程噪声，最后调零偏游走。因为观测噪声直接影响残差权重，是影响最大的一环。做法是固定其他参数，把观测噪声按十倍量级扫一遍，看轨迹的平滑度和滞后情况，找到拐点之后再细化。

注意：所有噪声参数的量纲必须和你的采样率匹配。连续时间的噪声密度要乘上采样间隔的平方根才能变成离散噪声方差。这一步漏掉的话，参数会差出一个数量级，怎么调都不对。我在第一个项目里就漏了这一步，导致所有噪声参数都被迫调到极小才勉强能用，实际上是量纲错了。

5.3 一个容易被忽略的细节：状态重置

迭代滤波在每次更新结束后，会把误差状态清零，把增量并入标称状态。这个“清零”动作在不同实现里的位置不一样，有的是框架自动做，有的是要求用户函数里做。如果你的实现需要手动做，漏掉的话，误差状态会越积越大，几帧之后残差就完全失去意义了。

判断方法很直接：在观测函数入口打印当前状态的某个分量，看它在两次迭代之间是否围绕同一个中心小幅波动。如果它每次迭代都在单调偏移，那就是没清零。

这个细节之所以值得单独说，是因为它不会导致崩溃，也不会报错，只会让精度慢慢变差。等你好不容易发现的时候，往往已经把模型的其他部分改得面目全非了。

6. 我个人在推导过程中的几点体会

先说一个方法论上的东西。我最早学这套框架，习惯从公式出发，把论文里的公式抄下来，然后一行行翻译成代码。这条路走得很痛苦，因为论文里的符号约定、坐标系定义、扰动定义，跟你手上的代码未必一致，你抄下来的公式很可能在第三步就符号反了。后来我换了个方式：先不看公式，先看代码里的boxplus和boxminus怎么实现，反推出它的约定，然后再按这个约定去推公式。速度一下就快了。因为流形上的所有推导，本质上都是围绕“扰动怎么定义”展开的，定义搞清楚了，剩下的就是机械的链式法则。

第二个体会是关于数值验证的。前面提过用数值微分验证雅可比，这个方法我要再强调一次，因为它实在太省时间了。具体做法是：写一个小测试，随机生成一个状态和一个小扰动，分别计算“解析雅可比乘扰动”和“残差的实际变化量”，两者相减取范数。如果解析推导正确，这个范数应该和扰动量成平方关系。你可以把扰动量从 1e-2 扫到 1e-8，如果误差范数不是按平方下降，说明某一项推导有误。这个测试可以覆盖所有雅可比，包括 F 和 W，二十分钟能全部跑完。

第三个体会是关于“够用就好”。前向推导在理论上可以做到非常精确，二阶项、科氏项、地球自转项全带上。但对绝大多数地面和室内场景，这些项的量级远小于传感器噪声，带上它们只是增加计算量和出错概率。我现在的做法是，先用最简模型跑通，跑通之后再用真实数据对比，看加上某一项能不能带来可观测的提升，能就加，不能就砍。这个原则帮我省掉了大量“为了严谨而严谨”的时间。

最后一个，关于文档和注释。这套框架的可读性不算高，宏展开之后基本没法调试。我的习惯是在每个用户函数开头，用注释把“输入是什么、输出是什么、维度是多少、约定是什么”写清楚，尤其是符号约定。半年后回头看自己的代码，这些注释能救命。

7. 后续可以继续深挖的几个方向

前向推导跑通之后，能往下走的路还挺多的，我列几个自己正在做的。

第一个是观测模型的扩展。点到面是最基础的，实际系统里还会用到点到线、点到点的组合，不同观测的雅可比结构不一样，但推导套路是一样的。把几种观测放在一起做联合更新时，要注意它们的噪声是独立的才能简单堆叠，不独立的话需要引入互协方差。

第二个是流形的自定义。框架允许你自己定义流形，只要实现那几个接口。比如你可以定义一个平面流形（位置加法向，四个自由度），或者定义一个带尺度的相似变换流形。自定义流形能让你把一些约束直接编码进状态，而不是事后投影，这在有强约束的场景里很有用。

第三个是退化检测。前向推导时如果能顺便算出观测雅可比的秩或者条件数，就可以在线判断当前观测是否充分。比如在长走廊里，点到面观测几乎全部来自两侧墙面，沿走廊方向完全没有约束，这时候如果还强行更新，就会在退化方向上产生错误的修正。加上退化检测之后，可以对这些方向做降权甚至冻结。

第四个是一致性与可观性分析。误差状态的递推关系写出来之后，其实可以分析哪些状态是可观的、哪些是不可观的。比如静止时航向角是不可观的，这时候如果强行去估计它，协方差会虚假收敛。理解这一点，能帮你判断某些“滤波发散”的现象其实是模型本身的问题，不是实现的问题。

这几条我都在陆续填坑，前向推导这一关过了之后，后面每往下走一步，都会反过来加深对这一关的理解。所以如果你现在正卡在某个雅可比对不上，别急着怀疑自己，大概率只是约定没对齐，静下心来做一次数值验证，问题通常比想象中简单。