调试状态估计器最让人抓狂的时刻，往往不是滤波器发散，而是它发散得"没有道理"——残差看着正常，观测也没问题，协方差却在一个安静的时间段里悄悄鼓起来。这种时候把日志倒回去翻，十有八九问题出在预测那一步：雅可比少了一项、零偏的 Δt 落错了位置，或者扰动的定义侧（左乘还是右乘）和更新部分对不上。IKFoM 这个模板库干的活，就是把这堆容易出错的推导，抽象成一套可以复用的流形工具，让滤波器主体只需要调用 boxplus 和 boxminus 就能工作。但它抽象得越干净，前向推导这部分就越值得单独拎出来捋一遍——因为一旦抽象层下面的约定和你脑子里的约定不一致，你会在后面几个数量级的调试时间里反复怀疑人生。

这篇笔记专门啃 IKFoM 的前向推导部分，也就是预测环节的数学骨架：状态怎么从流形上转移、误差状态怎么递推、协方差怎么传播、噪声怎么进模型。适合已经能跑通一个状态估计器、但对里面雅可比矩阵的每一项都说不清来路的人，也适合刚接触流形滤波、想从零把公式推一遍的新手。我不打算只贴结论矩阵，而是把每一步"为什么这么写"摊开讲，包括我自己推错过的几个地方。

1. 前向推导到底在推什么：先把问题边界划清楚

1.1 一次预测步骤只产出三样东西

很多人学滤波器的时候，脑子里是"预测—更新"两大块，但真到写代码的时候会发现预测这一块其实被切得很细。IKFoM 的前向推导部分，落到实处只做三件事：把名义状态从上一时刻推到当前时刻，把误差状态的转移关系线性化成一个矩阵，把这个转移关系作用到协方差上。三件事对应的输出物分别是状态量 x̂、状态转移矩阵 F、过程噪声协方差 Q。

这里有个非常关键的认知：F 和 Q 描述的不是"状态怎么变"，而是"误差怎么变"。滤波器内部真正在传播的是误差状态 δx 及其协方差，名义状态只是被携带的一个参考点。所以你推 F 的时候，泰勒展开的对象是误差量 δx，不是状态量 x 本身。我见过不少初学者直接拿动力学方程去对状态求偏导，写出来的东西维度都不对——因为对状态求导得到的是连续时间导数，而 F 是离散时间、跨一个采样周期的转移。这两个东西差着一个时间积分的层次。

还有一个容易混的点：F 到底是什么维度的矩阵。如果状态是一个由多个流形分量拼起来的复合流形，那么 F 的维度就是所有分量切空间维度之和。以常见的惯性导航状态为例，姿态是三维旋转群（切空间三维）、位置速度各三维、两个零偏各三维，加起来十五维，F 就是一个 15×15 的方阵。这个维度是切空间的维度，不是流形本身"看起来"的维度——比如四元数存了四个数，但它对应的切空间还是三维。

1.2 为什么不能用四元数直接做加减

新手最常见的疑问是：四元数不是可以直接加减吗，为什么非要绕一套指数映射？答案是四元数表示的旋转是单位四元数，它躺在一个三维球面（四维空间里的单位球）上，不在一个线性空间里。两个单位四元数相减，结果大概率不再是单位四元数，你得重新归一化。归一化这一步在滤波器里是致命的：它把误差的分布硬生生投影到球面上，破坏了高斯假设，协方差的正定性也会跟着出问题。

更麻烦的是"小角度近似"这一环。如果你直接在四元数上做线性加减，误差量的第一个分量（实部）和后面三个分量（虚部）之间的耦合关系就丢掉了，而协方差传播偏偏要靠这个耦合关系才能正确地对齐到切空间上。所以在流形上做滤波的核心操作不是加减，而是"在一个点处定义一个切空间，把误差放到切空间里，做完线性运算再映射回流形"。

这个思路推广开来就得到两个基本算子：从一个流形点出发，沿切空间里某个向量走一小步，回到流形上；以及反过来，两个流形点之间"差多少"，用切空间里的一个向量表示。这两个算子就是后面要反复出现的 boxplus 和 boxminus。只要你的状态能定义出这两个算子，整个滤波器的主体代码就完全不需要知道底层是旋转、是球面、还是欧氏空间。

1.3 推导的验证标准：数值差分能对上才算推对了

我在推这类公式时有个硬性习惯：只要写出一个雅可比块，立刻用数值差分验一遍。方法是给某个误差分量一个很小的扰动（比如 1e-6），前向走一步，看误差状态的变化量，和解析式算出来的 F 乘以扰动向量做比较，相对误差应该落在扰动尺度量级内。

这个习惯救过我很多次。有一次姿态块的符号写反了，解析式看起来"很像标准答案"，但数值差分立刻显示差了一个负号。还有一次是零偏项漏了 Δt 因子，量级上差了两个数量级，肉眼审公式根本看不出来，数值差分一秒暴露。这类错误如果流到实际的系统里，表现通常是"滤波器在长时间静止后突然漂移"或者"转弯时协方差异常增大"，排查成本高得多。

提醒：做数值差分时扰动不要太小。双精度下 1e-9 量级会被舍入误差吃掉，1e-6 到 1e-5 是比较安全的区间。同时要注意归一化操作的影响——如果你在差分时顺手做了四元数归一化，等于引入了非线性，差分结果会被污染。

2. 流形上的加减法：boxplus 与 boxminus 的工程语义

2.1 指数映射把切空间的直线映射成流形上的弧

先建立几何直觉。以旋转群为例，它的切空间在单位元处就是三维实数空间，里面的一个向量可以理解成"旋转轴乘以旋转角度"。指数映射做的事，是把这条从原点出发的直线，卷成流形上的一条测地线弧。走多远取决于这个向量的模长，模长就是旋转角。

这条弧的性质决定了小角度近似的适用范围。当模长很小（比如 1e-3 弧度以下），弧和直线几乎重合，指数映射可以近似成"单位阵加上反对称矩阵"这种极其廉价的形式。但当模长到了 0.1 弧度以上，二阶项开始显著，这个近似就不可用了。在滤波器的预测步骤里，每步的旋转增量通常是角速度乘以采样周期，量级在 1e-3 到 1e-2 之间，一阶近似绰绰有余；但在一些低频传感器或者高机动场景下，单步角度可能超过 0.1 弧度，这时候如果还用一阶近似，误差会以平方的速度累积。

这里要区分两件事：名义状态的递推，和误差状态的递推。名义状态用的是完整的指数映射（通常用查表或者三角函数实现），精度足够；误差状态做的是线性化，用一阶近似是合理的，因为线性化本身就是一阶截断。麻烦在于有些实现为了省算力，把名义状态的递推也换成一阶近似，这就把误差直接注入了参考轨迹，长此以往会出现系统性偏差。我在早期自己写的版本里就干过这事，表现是静止状态下姿态慢慢"爬"，一小时爬出零点几度。

2.2 boxplus 不是向量加法，它有三条硬约束

把"沿切空间走一步"这件事实现成函数，就是 boxplus(x, δ)。要让它在滤波器里可用，至少得满足三条约束。

第一是封闭性：boxplus 的结果必须还是同一个流形上的点，不能跑出去。对旋转来说意味着结果仍然是正交阵；对单位球面来说意味着结果仍然是单位向量。如果实现里用了近似，封闭性可能被破坏，所以在实现里通常会显式做一次归一化或者正交化来"拉回来"。

第二是可逆性：boxminus(boxplus(x, δ), x) 应该近似等于 δ。注意这里是"近似"，因为流形是弯的，走一个来回会有高阶残差。这个残差的量级决定了滤波器在高频预测下的精度上限。如果可逆性很差，误差状态在预测和更新之间来回转换时会引入伪噪声，协方差会虚高。

第三是光滑性：在 δ = 0 附近必须有足够好的连续性和可导性。这一条在实现中容易被忽略。有些流形在奇异点附近（比如球面的极点、旋转角接近 π）雅可比会退化，滤波器在那里会表现得非常不稳定。IKFoM 里对球面流形做了特殊处理来避开极点问题，这部分逻辑在推导时也要一并考虑。

2.3 拿旋转手推一遍，看清楚近似到底丢掉了什么

设名义旋转为 R̂，误差旋转向量为 δθ。右扰动约定下，真值旋转写成 R = R̂ · Exp(δθ)。展开 Exp(δθ) 到一阶：

Exp(δθ) ≈ I + [δθ]×

其中 [·]× 表示把三维向量变成反对称矩阵。于是

R ≈ R̂ + R̂[δθ]×

把它作用到一个向量 a 上：

R a ≈ R̂ a + R̂ (δθ × a) = R̂ a - R̂ [a]× δθ

这个式子是后面所有推导的地基。它说明：旋转的误差对向量的影响，是通过"向量自己的反对称矩阵"来耦合的。注意这个负号，很多符号错误都出在这里。如果你写成 R̂ a + R̂[a]× δθ，那就是把叉乘的顺序搞反了（δθ × a = -a × δθ）。

同样的，二阶项的量级是 |δθ|²。在 δθ 为 1e-2 时，二阶项约 1e-4，相对于一阶项 1e-2 是百分之一的相对误差。这个误差会以"偏差"的形式进入协方差，不会立刻爆炸，但会在长时间的静默期里慢慢累积成不可忽视的量。所以如果有人在长时间运行后发现姿态协方差单调增长，除了检查噪声参数，也要看看离散化阶次是不是太低了。

3. 误差状态递推的通式：把推导变成可复用的套路

3.1 名义轨迹与真值轨迹的分离

流形滤波的标准做法是把状态拆成两条线：一条是滤波器维护的名义状态，它没有噪声，按照确定性的模型递推；另一条是理论上存在的真值状态，它包含了所有噪声和模型误差。两者之间的差就是误差状态，用 boxminus 表示。

这个拆分不是形式主义，它有一个非常实际的收益：误差状态始终在零附近，所以围绕它做的线性化永远是"在原点附近"的线性化，线性化点不会跑远。如果直接对状态做线性化，随着状态值变大（比如位置跑到了几十万米），线性化的相对精度会明显下降。这也是为什么流形滤波比直接在状态空间做扩展卡尔曼要稳定得多。

具体到实现上，预测分成两遍算。第一遍用名义模型把状态推一步，得到新的名义状态；第二遍把误差状态代入，做泰勒展开，得到转移矩阵 F。这两遍在代码里通常对应两个不同的函数，IKFoM 里体现为状态预测回调和雅可比回调的分离。新手常犯的错误是试图在一个函数里同时干这两件事，结果两个逻辑搅在一起，调试时根本分不清是状态推错了还是雅可比错了。

3.2 三步固定动作：代入、配平、展开

我推误差状态递推时固定走三步，这套动作几乎对所有流形都适用。

第一步是代入。把真值状态用名义状态和误差量表示，代入真实的递推方程；把名义状态代入名义递推方程。注意真实方程里带噪声项，名义方程里噪声置零。

第二步是配平。目标是凑成"真值递推结果 = 名义递推结果 ⊞ 新的误差"这种形式。这一步是最需要技巧的，因为流形上的乘法不满足交换律，误差量可能出现在表达式的左边也可能在右边。右扰动约定要求误差始终在右边，如果推导过程中误差跑到了左边，就得用一次"左扰转右扰"的恒等变换把它挪过去。旋转群上的变换关系是 R̂ · Exp(δ) · R̂ᵀ = Exp(R̂ δ)，反过来 Exp(δ) · R̂ = R̂ · Exp(R̂ᵀ δ)。这一步用错了，符号就会全线崩盘。

第三步是展开。用贝克-坎贝尔-豪斯多夫公式的一阶截断，把两个指数映射的乘积合并成一个。对旋转来说，一阶形式是 Exp(a)Exp(b) ≈ Exp(a + b + 0.5 · a × b)。把噪声项和误差项都当作小量，做一阶截断，消去两边的公因子，就得到了新的误差表达式。

这套动作的价值在于可复用。换一个流形、换一个动力学方程，三步还是这三步，只是中间的代数细节不同。我建议每个做流形滤波的人都把这套流程默写下来，比背任何具体的结果矩阵都有用。

3.3 误差定义在哪一侧，决定了整张 F 的长相

左右扰动的选择不是审美问题，它直接决定 F 矩阵的形态，也决定和更新部分的兼容性。

右扰动下，误差固定在状态右侧，姿态误差的转移矩阵里会出现一个"旋转后的角速度"的反对称矩阵项，位置和速度对姿态的偏导里出现的是旋转矩阵本身而不是它的转置。左扰动下，这些项会多出旋转矩阵的转置，看起来更"绕"，但它在某些推导里更顺手，因为世界坐标系下的量可以直接作用在左边。

真正的危险在于混用。假设预测部分的 F 是用右扰动推出来的，而更新部分的观测雅可比是用左扰动推出来的，两边各自都对，合在一起就是错的。表现是滤波器在静态时看上去还稳定，一旦有旋转运动，状态就会被观测"拽"向错误方向，或者残差呈现明显的系统性偏差。这种 bug 极难定位，因为单看每一部分的公式都符合教科书。

我的建议是：一旦选定一种约定，就在整个工程里用注释把它钉死，所有涉及扰动的函数入口都注明"右扰动"或"左扰动"，并且在最早的单元测试里就验证两者的组合行为。IKFoM 在这一点上做了统一，它要求你通过 boxplus 和 boxminus 明确定义扰动的作用方式，滤波器主体不再关心方向问题，这本身就是一种防错设计。

4. 以惯性递推为例，把 F 矩阵逐块摊开

4.1 状态定义与离散递推方程

为了让推导有具体抓手，选一个最经典的场景：惯性导航状态的预测。状态取 15 维：姿态 R（三维旋转群）、位置 p（三维欧氏）、速度 v（三维欧氏）、陀螺零偏 b_g（三维欧氏）、加计零偏 b_a（三维欧氏）。下面统一按右扰动约定，记真值与名义量的关系为 R = R̂ · Exp(δθ)，其余分量写成真值 = 名义值 + 误差。

连续时间动力学写成：

Ṙ = R · [ω - b_g - n_g]× v̇ = R · (a - b_a - n_a) + g ṗ = v ḃ_g = n_bg ḃ_a = n_ba

离散化用一阶欧拉，采样周期记作 Δt，记 ω̂ = ω - b̂_g、â = a - b̂_a：

R̂' = R̂ · Exp(ω̂ Δt) v̂' = v̂ + (R̂ â + g) Δt p̂' = p̂ + v̂ Δt + 0.5 (R̂ â + g) Δt² b̂_g' = b̂_g b̂_a' = b̂_a

零偏在预测步骤里保持不变，这是"随机游走"模型的直接体现——它的变化完全由噪声驱动，确定性部分为零。这一点在后面写 F 的时候会体现为两个单位阵。

4.2 姿态误差块：一阶近似下的那半个雅可比

先把姿态的真值递推写出来，噪声项保留：

R' = R · Exp((ω - b_g - n_g) Δt)

代入 R = R̂ Exp(δθ)、b_g = b̂_g + δb_g：

R' = R̂ Exp(δθ) · Exp((ω̂ - δb_g - n_g) Δt)

用一阶的贝克-坎贝尔-豪斯多夫公式合并两个指数：

Exp(δθ) Exp(u Δt) ≈ Exp(δθ + u Δt + 0.5 · (δθ × u Δt))

其中 u = ω̂ - δb_g - n_g。把这个结果和名义递推 R̂' = R̂ Exp(ω̂ Δt) 对齐，两边同时左乘 R̂ᵀ：

Exp(δθ + u Δt + 0.5 (δθ × u) Δt) = Exp(ω̂ Δt) Exp(δθ')

这里右边再展开一次，把 Exp(ω̂ Δt) Exp(δθ') 合起来。但更简洁的做法是把左边的一阶项化简后直接对比指数上的向量（因为都在小量范围内，指数映射在小量邻域内是单射）。左边指数上的向量展开，把 u 代回去，注意 δθ × (ω̂ Δt) 这一项：

δθ + ω̂ Δt - δb_g Δt - n_g Δt + 0.5 (δθ × ω̂) Δt

利用叉乘的反对称性质，δθ × ω̂ = -ω̂ × δθ = -[ω̂]× δθ，所以这一项可以写成 -0.5 [ω̂ Δt]× δθ。整理后得到：

δθ' = (I - 0.5 [ω̂ Δt]×) δθ - Δt δb_g - Δt n_g

这就是姿态误差块的完整结论。三块分别是：姿态自身的转移（那个带 0.5 的反对称矩阵）、零偏对姿态的耦合（负 Δt 单位阵）、噪声项。那个 0.5 的因子是很多人写错的地方——有人的实现里没有它，短期看不出问题，但在高角速度下会引入明显的相位误差。

4.3 速度、位置与零偏块的推导

速度的真值递推：

v' = v + (R (a - b_a - n_a) + g) Δt

把 R 用一阶近似展开成 R̂ (I + [δθ]×)，再作用到 (â - δb_a - n_a) 上。关键的一步是那个负号：

R̂ (I + [δθ]×)(â - δb_a - n_a) ≈ R̂ â - R̂ [â]× δθ - R̂ δb_a - R̂ n_a

这里用到了 [δθ]× â = δθ × â = -â × δθ = -[â]× δθ。整理后：

δv' = δv - R̂ [â]× Δt · δθ - R̂ Δt · δb_a - R̂ Δt · n_a

位置块的推导路径完全一样，只是多了一个 0.5 的系数和一项速度误差：

δp' = δp + Δt · δv - 0.5 R̂ [â]× Δt² · δθ - 0.5 R̂ Δt² · δb_a - 0.5 R̂ Δt² · n_a

零偏块最简单：

δb_g' = δb_g + Δt · n_bg δb_a' = δb_a + Δt · n_ba

把这些拼起来，按 [δθ, δp, δv, δb_g, δb_a] 的顺序排列，就得到完整的 15×15 状态转移矩阵。下面这张表把每一块的来源和量级列清楚，方便对着代码核对：

4.4 一个反直觉的细节：位置块的 Δt² 从哪里来

不少人第一次看到位置对零偏的偏导是 0.5 R̂ Δt² 会觉得奇怪——为什么是平方而不是一次方？直观解释是：零偏误差先影响加速度，加速度积分一次得到速度误差，速度再积分一次得到位置误差，两次积分各贡献一个 Δt。而 0.5 这个系数来自二次积分的梯形公式：加速度恒定假设下，位置增量是 0.5 a Δt²。

这个细节的实际意义在于量级。当 Δt 是 0.005 秒（200Hz）时，Δt² 是 2.5e-5，比 Δt 小了 200 倍。所以位置对零偏的耦合在短周期预测里几乎可以忽略，但在低频（比如 20Hz，Δt = 0.05）时，Δt² = 2.5e-3，相对贡献上升到千分之一，就不能随便丢了。我见过有人为了"简化计算"把所有 Δt² 项直接删掉，在 200Hz 下跑得挺好，换成 50Hz 的传感器立刻出现明显的尺度漂移——原因就在这里。

顺便说一句离散化的阶次。上面用的是显式欧拉，误差是 O(Δt²) 量级。如果想要更精确，可以用中点法或者龙格库塔，代价是每一步要多次计算动力学。对绝大多数惯性传感器来说，一阶欧拉在 100Hz 以上已经够用，因为传感器噪声本身比离散化误差大得多。但如果你的应用需要和低频传感器对齐，或者要做精细的离线回放，提升积分阶次是有意义的。

5. 噪声怎么进误差状态：G 矩阵与 Q 的拼装

5.1 连续噪声谱密度到离散协方差的尺度变化

噪声参数是最容易被随手填的东西，也是滤波器表现最敏感的旋钮。常见做法是从传感器手册上抄一个数填进去，然后发现滤波器要么过度自信、要么完全迟钝。问题往往出在量纲和尺度上。

传感器手册给的通常是连续时间的噪声密度，单位是"每根号赫兹"，比如陀螺的角度随机游走单位是度每根号小时。而滤波器里需要的是离散时间的噪声方差，单位是"平方每步"。两者之间的换算关系是一条铁律：离散方差 = 连续谱密度² × Δt。注意是谱密度的平方乘一次 Δt，不是平方再乘 Δt 的平方。

这个换算关系经常被写错。写成 Δt² 的话，在 200Hz 下会让噪声被低估 200 倍，滤波器变得极度自信，观测一有偏差就会被猛烈拉扯，表现为状态跳动。写成不乘 Δt 的话，噪声被高估，滤波器会过度平滑，响应迟钝，动态场景下出现明显的滞后。我调试过一个系统，就是因为这个换算写成了平方，静止时看不出问题，一上动态就跟不上，排查了整整两天。

5.2 G 矩阵逐块写

噪声进入误差状态的方式，是把每个噪声源对应到它影响的误差分量上。把噪声向量按 [n_g, n_a, n_bg, n_ba] 排列，从第 4 节的推导里直接抄出系数：

这张表就是 G 矩阵（15×12）。有了 G，过程噪声协方差就是 Q = G · diag(σ_g², σ_a², σ_bg², σ_ba²) · Gᵀ。

这里有个可以省算力的地方：R̂ Δt 这种项，因为 R̂ 是正交阵，所以 (R̂ Δt)(R̂ Δt)ᵀ = Δt² · I。也就是说 Q 里面旋转矩阵会被消掉，最终 Q 的形式非常干净，只和标量噪声参数以及 Δt 的幂次有关。这是个很实用的性质，意味着你可以在推导阶段就把 Q 化简成对角块形式，运行时不用做任何矩阵乘法。IKFoM 这类模板库在编译期就把这些结构确定下来了，运行时的开销主要花在 F 的乘法上。

5.3 Q 的对称性和正定性不能靠运气

Q 必须对称正定，否则协方差传播后可能失去正定性，滤波器会直接崩。理论上是满足的（因为它是 G 乘对角阵乘 Gᵀ 的形式），但数值上不一定。

问题出在参数极端的时候。如果某个噪声参数填成了 0，那一行对应的协方差就是 0，整个 Q 变成半正定，某些方向上就失去了约束，卡尔曼增益计算里的求逆可能出问题。实践中我建议所有噪声参数都给一个非零的下限，哪怕传感器精度极高，也给个 1e-8 之类的数兜底。

另一个隐患是累积误差。随着迭代次数增加，协方差的对称性会因为浮点误差逐渐被破坏。标准做法是每次传播后强制做一次对称化：P = 0.5 (P + Pᵀ)。这个操作成本极低，但能避免很多莫名其妙的数值崩溃。我在一个长时运行的场景里遇到过，跑了八小时后协方差出现极小的负特征值导致求逆失败，加上对称化之后就没再复现。

注意：对称化只是补丁，不是解决方案。如果协方差经常需要靠对称化来维持正定，说明 F 矩阵本身有问题，或者噪声参数严重不合理。先查根因，再谈加固。

6. IKFoM 的模板封装把推导藏在了哪

6.1 状态是流形的笛卡尔积，不是一堆孤立变量

IKFoM 最核心的设计思路是：把整个状态看成一个复合流形，也就是若干个子流形的笛卡尔积。姿态是旋转群、位置速度是三维欧氏空间、零偏也是三维欧氏空间，把它们拼在一起，整体仍然是一个流形。既然是流形，就只需要提供 boxplus 和 boxminus 两个操作，滤波器主体就能工作。

这个抽象带来的直接好处是，滤波器的预测和更新代码不需要知道状态里到底是哪些量。它只负责"调用 boxplus 把状态往前走一步"、"调用 boxminus 算残差"、"按模板参数分配矩阵维度"。真正的领域知识被推到了两个地方：状态的定义（哪些分量、什么类型），以及用户自己写的动力学函数和雅可比函数。

顺着这个思路，库提供了一套构建机制：用宏或者模板把若干流形类型声明成一个状态类型，自动生成该状态的切空间维度、boxplus、boxminus。对每个子流形，只需要实现自己的两个算子，复合时逐分量应用即可。

6.2 雅可比到底是手写还是自动求

这是初学者最容易困惑的地方。boxplus 和 boxminus 自动生成没问题，但 F 矩阵涉及对动力学函数求雅可比，这一步库能自动做吗？

答案是：取决于你怎么用。库通常提供两条路。第一条是让你手写雅可比回调，把第 4 节里推出来的那些块按顺序填进去。第二条是提供一个数值差分的通用实现，通过多次调用动力学函数来近似雅可比。前者的精度和速度都更好，后者开发快但有代价。

数值差分的代价不只是慢，还有精度。因为差分步长的选择和状态量级有关，位置在几万米时和在地面附近时，合适的步长完全不同。固定步长的差分在状态量级变化大的场景下会出现精度退化。而且差分对动力学函数的光滑性要求很高，如果你的动力学里有硬阈值或者查表插值，差分结果会带噪声，进而污染协方差。

我自己的做法是：开发阶段用数值差分快速跑通，验证整个滤波器框架没问题；然后逐块替换成解析雅可比，每替换一块就用差分做一次对照验证。这样既保证了开发速度，又保证了最终精度，还能在替换过程中发现很多隐藏的符号问题。

6.3 编译期展开的收益与代价

模板库的一个显著特点是所有维度都确定在编译期，运行时的矩阵维度是常量，编译器可以把循环完全展开，甚至把稀疏结构优化掉。对嵌入式平台来说这个收益很大，因为动态内存分配和循环开销都被消除了。

代价是编译时间和二进制体积。状态维度越大、复合流形层次越多，模板实例化的组合就越多，编译时间会明显增加。我在一个包含多种传感器配置的项目里遇到过编译时间从几十秒涨到几分钟的情况，最后是通过拆分成独立的编译单元缓解的。

另一个代价是错误信息的可读性。模板错误信息通常会展开好几屏，真正的错误藏在最后几行。我的经验是，遇到模板报错先从最后一行往前看，找第一个提到具体类型名的地方，那通常就是问题所在。另外，状态定义里的括号配对错误是最常见的，宏展开之后报错位置会非常误导。

7. 我在前向推导里踩过的坑

7.1 左右扰动混用：最隐蔽的符号灾难

前面已经提过这个问题，但值得单独展开，因为它的隐蔽程度远超其他错误。

具体表现是这样的：预测部分用右扰动推出 F，更新部分用左扰动写观测雅可比。单独测试预测，协方差传播正常；单独测试更新，状态修正量也符合预期。但合在一起运行，会看到残差呈现出与运动方向相关的系统性偏差——直行时还好，一转弯就偏。

排查的难点在于，两部分的代码单独看都没毛病，单元测试也都能过。我当时的做法是把预测步骤的误差定义打印出来，手动构造一个已知的小扰动，走一步预测，然后看残差和理论值对不对。这一步立刻发现更新部分算出来的残差方向是反的，因为它假设误差在左边。

从那以后我养成了一个习惯：在状态的头文件里用一行注释明确写死扰动约定，任何新加入的模块都必须对齐这个约定，代码评审时把这一条当作硬性检查项。

7.2 离散化阶次和协方差膨胀的因果关系

有一次调试一个长时间运行的场景，发现协方差在静止状态下单调增长，位置标准差从厘米级慢慢涨到米级，但实际状态并没有漂移。这说明协方差传播过保守了。

追下去发现两个原因叠加。第一，积分用的显式欧拉，但 Δt 的取值是传感器周期，而实际预测是以更高频率被调用的（因为还有别的触发源），导致每个小周期都注入了一份完整的离散噪声，噪声被重复计数。第二，Q 的计算里没有考虑"预测步和观测步频率不一致"的情况，观测频率远低于预测频率时，两次观测之间的噪声累积被低估了。

修复方法是把噪声注入改成按实际经过的时间成比例缩放，而不是按调用次数。这个改动看起来简单，但它要求你在预测函数里传入真实的时间差，而不是默认一个常数。很多实现为了简化都在这里写死了 Δt，短期没问题，一旦有丢帧或者变频率就会出问题。

7.3 零偏项漏 Δt：量级错误的典型样本

这是个听起来很蠢但实际发生率极高的错误。零偏对姿态的耦合项应该是 -Δt 乘以单位阵，写过的人有相当比例会漏掉 Δt。

原因在于推导的时候，很多人是分两步想的：先在连续时间下写出关系，再做离散化。连续时间下，姿态误差对零偏的偏导就是单位阵（因为角速度误差等于零偏误差），离散化之后才乘上 Δt。如果推导过程中断在中间某一步，很容易把连续时间的结果当成最终答案。

这个错误的症状很有辨识度：滤波器在静止时收敛正常，一旦有持续旋转，姿态会明显滞后于真值。因为零偏修正量被放大了约 1/Δt 倍（在 200Hz 下是 200 倍），滤波器对零偏的估计会过度激进，表现就是姿态跟踪不上快速的角速度变化。我第一次遇到时以为是噪声参数问题，调了半天才发现是量级错了。

7.4 验证方法：把差分测试写成脚本

前面说过要用数值差分验证雅可比，这里给一个具体可操作的做法。

写一个测试脚本，做三件事：随机生成一个名义状态和一个小扰动；用数值差分算出 F 的某一列（对某个误差分量施加扰动，看输出误差的变化）；和解析式算出的对应列做比较，输出相对误差。

关键是覆盖率。不要只测一个随机的状态，要把姿态设成接近零、接近 π、任意角度三种情况，把速度设成零和高速两种情况，把零偏设成零和非零两种情况，做笛卡尔积。我就是在"姿态接近 π"这一档里发现过指数映射的奇异处理有问题，实际运行中几乎不可能随机撞到那个点，但理论上有风险。

对于噪声矩阵 G，验证思路是把 Q 和直接用噪声样本做蒙特卡洛仿真的结果对比。生成几千组噪声样本，按 G 作用到误差状态上，算经验协方差，和解析 Q 比较。这个验证成本高一些，但能一次性确认所有噪声项的系数和量纲都对了，非常值得。

写到这里，前向推导这部分的主干基本铺完了。真正把这套推导吃透的标志，不是能默写出那张 15×15 的矩阵，而是拿到任何一个新的流形、任何一个新的动力学模型，都能按"代入、配平、展开"这三步把 F 和 G 重新搭出来。我现在遇到新传感器的融合需求，第一件事就是把它的动力学写成流形形式，然后走一遍这三步，比去翻任何现成的公式都快。