把 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各成员存储维之和切空间维 n误差状态的自由度23各成员自由度之和协方差维P 矩阵的阶数n × n等于切空间维过程噪声维 m噪声向量的长度12传感器噪声源个数观测维 k单次观测残差长度视观测而定观测模型决定状态雅可比F 矩阵n × n切空间维噪声雅可比W 矩阵n × m切空间维 × 噪声维观测雅可比H 矩阵k × n观测维 × 切空间维表里最反直觉的是“流形存储维”和“切空间维”不相等。以上面的状态为例两个旋转各多存一个数、方向向量多存一个数加起来存储维比切空间维多三个。协方差矩阵是切空间维的方阵所以它比“把结构体拍平”得到的维度要小。如果你按存储维去开协方差矩阵编译期可能不报错因为模板参数是你自己填的运行起来就是内存越界或者静默错位。我在一个项目里就吃过这个亏手动写了一个 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 五个典型现象与排查路径下面这张表是我在实际项目里积累的按“现象—可能原因—验证方法”组织遇到问题可以直接对照着查。现象可能原因验证方法静止时速度缓慢漂移重力项符号或坐标系错误静止采集一分钟看速度均值是否单调转弯时姿态估计滞后姿态误差与角速度的耦合项符号反了单向匀速转弯对比陀螺积分零偏估计不收敛噪声雅可比与 Q 顺序不一致单轴激励观察该轴零偏响应长时间运行后滤波发散P 失去对称性或接近奇异每帧检查 P 的对称性和最小特征值迭代次数总是打满观测雅可比有误或残差量级失衡数值微分验证雅可比第一条我要特别展开。静止漂移是最容易被误判成“零偏没标定好”的问题。判断方法很简单把载体完全静止放在水平面上采集一分钟如果速度估计单调朝一个方向走那就是重力项的问题如果速度在零附近上下抖那才是噪声问题。前者需要改公式后者调参数就行。第四条也值得展开。P 矩阵在长时间运行后失去正定性是一个非常经典的数值问题。除了前面说的对称化和加小量还有一个办法是在每次更新后做一次特征值截断把小于零的特征值强制置为一个小正数。这个操作代价不低不建议每帧都做可以每隔若干帧做一次或者只在检测到异常时做。5.2 一份可以直接抄的参数起点参数没有普适值但有一个合理的起点能省掉大量试错。下面这份表是我在几个不同平台上收敛比较快的初始配置你可以从这里出发再微调。参数建议初值调整方向陀螺白噪声1e-4 rad/s/√Hz 量级静态抖动大就调大加计白噪声1e-3 m/s²/√Hz 量级静止速度抖就调大陀螺零偏游走1e-6 量级零偏收敛慢就调大加计零偏游走1e-5 量级同上点到面观测噪声0.01 到 0.1 米平面拟合差就调大迭代收敛阈值1e-4精度不够就调小迭代上限3 到 5一般不用改调参的顺序建议是先调观测噪声再调过程噪声最后调零偏游走。因为观测噪声直接影响残差权重是影响最大的一环。做法是固定其他参数把观测噪声按十倍量级扫一遍看轨迹的平滑度和滞后情况找到拐点之后再细化。注意所有噪声参数的量纲必须和你的采样率匹配。连续时间的噪声密度要乘上采样间隔的平方根才能变成离散噪声方差。这一步漏掉的话参数会差出一个数量级怎么调都不对。我在第一个项目里就漏了这一步导致所有噪声参数都被迫调到极小才勉强能用实际上是量纲错了。5.3 一个容易被忽略的细节状态重置迭代滤波在每次更新结束后会把误差状态清零把增量并入标称状态。这个“清零”动作在不同实现里的位置不一样有的是框架自动做有的是要求用户函数里做。如果你的实现需要手动做漏掉的话误差状态会越积越大几帧之后残差就完全失去意义了。判断方法很直接在观测函数入口打印当前状态的某个分量看它在两次迭代之间是否围绕同一个中心小幅波动。如果它每次迭代都在单调偏移那就是没清零。这个细节之所以值得单独说是因为它不会导致崩溃也不会报错只会让精度慢慢变差。等你好不容易发现的时候往往已经把模型的其他部分改得面目全非了。6. 我个人在推导过程中的几点体会先说一个方法论上的东西。我最早学这套框架习惯从公式出发把论文里的公式抄下来然后一行行翻译成代码。这条路走得很痛苦因为论文里的符号约定、坐标系定义、扰动定义跟你手上的代码未必一致你抄下来的公式很可能在第三步就符号反了。后来我换了个方式先不看公式先看代码里的boxplus和boxminus怎么实现反推出它的约定然后再按这个约定去推公式。速度一下就快了。因为流形上的所有推导本质上都是围绕“扰动怎么定义”展开的定义搞清楚了剩下的就是机械的链式法则。第二个体会是关于数值验证的。前面提过用数值微分验证雅可比这个方法我要再强调一次因为它实在太省时间了。具体做法是写一个小测试随机生成一个状态和一个小扰动分别计算“解析雅可比乘扰动”和“残差的实际变化量”两者相减取范数。如果解析推导正确这个范数应该和扰动量成平方关系。你可以把扰动量从 1e-2 扫到 1e-8如果误差范数不是按平方下降说明某一项推导有误。这个测试可以覆盖所有雅可比包括 F 和 W二十分钟能全部跑完。第三个体会是关于“够用就好”。前向推导在理论上可以做到非常精确二阶项、科氏项、地球自转项全带上。但对绝大多数地面和室内场景这些项的量级远小于传感器噪声带上它们只是增加计算量和出错概率。我现在的做法是先用最简模型跑通跑通之后再用真实数据对比看加上某一项能不能带来可观测的提升能就加不能就砍。这个原则帮我省掉了大量“为了严谨而严谨”的时间。最后一个关于文档和注释。这套框架的可读性不算高宏展开之后基本没法调试。我的习惯是在每个用户函数开头用注释把“输入是什么、输出是什么、维度是多少、约定是什么”写清楚尤其是符号约定。半年后回头看自己的代码这些注释能救命。7. 后续可以继续深挖的几个方向前向推导跑通之后能往下走的路还挺多的我列几个自己正在做的。第一个是观测模型的扩展。点到面是最基础的实际系统里还会用到点到线、点到点的组合不同观测的雅可比结构不一样但推导套路是一样的。把几种观测放在一起做联合更新时要注意它们的噪声是独立的才能简单堆叠不独立的话需要引入互协方差。第二个是流形的自定义。框架允许你自己定义流形只要实现那几个接口。比如你可以定义一个平面流形位置加法向四个自由度或者定义一个带尺度的相似变换流形。自定义流形能让你把一些约束直接编码进状态而不是事后投影这在有强约束的场景里很有用。第三个是退化检测。前向推导时如果能顺便算出观测雅可比的秩或者条件数就可以在线判断当前观测是否充分。比如在长走廊里点到面观测几乎全部来自两侧墙面沿走廊方向完全没有约束这时候如果还强行更新就会在退化方向上产生错误的修正。加上退化检测之后可以对这些方向做降权甚至冻结。第四个是一致性与可观性分析。误差状态的递推关系写出来之后其实可以分析哪些状态是可观的、哪些是不可观的。比如静止时航向角是不可观的这时候如果强行去估计它协方差会虚假收敛。理解这一点能帮你判断某些“滤波发散”的现象其实是模型本身的问题不是实现的问题。这几条我都在陆续填坑前向推导这一关过了之后后面每往下走一步都会反过来加深对这一关的理解。所以如果你现在正卡在某个雅可比对不上别急着怀疑自己大概率只是约定没对齐静下心来做一次数值验证问题通常比想象中简单。