从卡尔曼滤波到信息滤波：多传感器融合的状态估计新思路

卡尔曼滤波这套东西，做过状态估计的人都能背出那五个方程，但真正在多传感器融合的工程现场泡过几年之后，你会发现标准形式在某些场景下会变得特别别扭。信息滤波（Information Filter）就是冲着这种别扭来的——它把高斯分布从均值加协方差的写法，换成信息向量加信息矩阵的写法，换完之后观测更新变成了一句加法，多传感器往同一个状态估计里塞观测时几乎不需要任何协调。代价也明摆着：预测步骤从一行公式变成一个矩阵求逆。这笔买卖划不划算，取决于你的系统是观测密集还是预测密集，是单机跑还是分布式跑。下面我把整套推导从头捋一遍，会讲清楚每一步为什么要这么走、哪些地方容易卡壳，以及用 numpy 复现时我自己踩过的坑。

1. 为什么值得把卡尔曼滤波换个写法

1.1 从一次多传感器融合的麻烦说起

假设你手上有一台移动平台，上面挂着激光雷达、轮式里程计、IMU，还有一个视觉模块。每个传感器都有自己的观测模型和噪声特性，你需要把它们的信息融进同一个状态估计里。用标准卡尔曼滤波做这件事，流程是这样的：先做一次预测得到先验均值和先验协方差，然后拿第一个传感器的观测做一次更新，得到后验；再拿第二个传感器的观测对刚才的后验再做一次更新，得到新的后验；第三个、第四个依此类推。

这个串行流程本身没错，数学上完全等价于把四个观测拼成一个大观测向量一次性更新。问题出在工程实现上。每一次更新都要计算卡尔曼增益，而计算增益绕不开对创新协方差矩阵求逆，那个矩阵的维度等于该传感器的观测维度。四个传感器就要做四次求逆，而且这四次更新必须严格按顺序执行，前一个的结果是后一个的输入。如果你想把这四个传感器的处理拆到四个进程或者四块板子上并行跑，就会非常难受，因为下游必须等上游算完。更麻烦的是，如果某个传感器临时掉线，整个更新链的中间态需要重新组织，代码里到处是分支判断。

信息滤波解决的正是这个结构性问题。在信息形式下，每个传感器的贡献是一份独立的加项，把它们加起来就完事，加法的顺序无所谓，谁先算完谁先加，掉线了不加就行，不需要任何补偿逻辑。这个性质在分布式估计、传感器网络、多机器人协同定位里是决定性的。

1.2 信息矩阵与协方差矩阵的对偶关系

要理解信息滤波，先要接受一个事实：描述一个高斯分布，均值和协方差不是唯一的选择。协方差矩阵 Σ 描述的是不确定性有多大，对角线元素越大表示越不确定。而它的逆矩阵 Λ = Σ⁻¹ 描述的是确定性有多强，数值越大表示该方向上的约束越紧。后者就是所谓的信息矩阵，有些文献也叫精度矩阵。

这种对偶不只体现在符号上，它对应着两种完全不同的物理直觉。协方差视角下，你关心的是误差的散布范围，一个很大的 Σ 意味着你对状态几乎一无所知。信息视角下，你关心的是各个方向被约束得有多死，Λ 的某个特征值很大，说明在这个特征向量方向上你有非常硬的证据。举个生活化的例子：协方差像是地图上画的一个模糊圈，圈越大越不确定；信息矩阵像是往这个圈上钉的钉子，钉子越多、钉得越深，圈就被压缩得越小。观测更新在协方差视角下是"缩小模糊圈"，在信息视角下就是"往圈上钉钉子"，而钉钉子这个动作天然是可叠加的。

理解了这一层，后面观测更新为什么是加法就顺理成章了——每来一个观测，无非是多钉一颗钉子，钉子之间互不干扰，总效果就是把各自的贡献加起来。

1.3 什么场景该果断换到信息形式

不是所有问题都适合用信息滤波。我自己的判断标准大致是这样几条。如果你的观测更新频率远高于预测频率，比如一个静止的传感器网络每隔几毫秒上报一次数据，而系统状态本身变化很慢，那么更新便宜、预测贵的特性就非常划算。如果你需要把估计任务分散到多个计算单元上，或者需要支持任意传感器的即插即用，那基本没有别的选择。如果你的观测数量非常大但每个观测维度很低，比如几千个测距基站各给一个标量距离，信息形式的优势会被放大到极致，因为每个基站贡献的是一个秩一矩阵的加法。

反过来，如果你的系统是预测密集型，比如高频率的惯性递推，观测偶尔才来一次，那标准卡尔曼形式明显更省算力。另外，如果你的状态维度远大于观测维度，信息矩阵会变成一个巨大的稠密矩阵，而协方差形式在观测少的时候反而更容易保持稀疏结构，这时候也不该硬上信息滤波。

2. 高斯分布的信息形式：从指数配方说起

2.1 标准形式到信息形式的等价改写

多变量高斯分布的标准写法是

$$ p(x) = (2\pi)^{-n/2} |\Sigma|^{-1/2} \exp\left(-\frac{1}{2}(x-\mu)^T \Sigma^{-1} (x-\mu)\right) $$

现在做两件事：令 Λ = Σ⁻¹，令 η = Σ⁻¹μ。把指数里的二次型展开：

$$ -\frac{1}{2}(x-\mu)^T \Lambda (x-\mu) = -\frac{1}{2}x^T\Lambda x + x^T \Lambda \mu - \frac{1}{2}\mu^T \Lambda \mu $$

最后那一项与 x 无关，可以并进归一化常数里。注意 x^TΛμ 里的 Λμ 正好就是 η。于是整个分布可以重写成

$$ p(x) \propto \exp\left(-\frac{1}{2}x^T\Lambda x + x^T\eta\right) $$

这就是信息形式。它看起来只是换了个写法，但结构上的变化很关键：指数项里 x 的二次项系数是 Λ，一次项系数是 η，两者完全解耦，没有任何需要求逆的操作。归一化常数虽然涉及 |Σ| 的行列式，但在做贝叶斯更新时，我们往往只关心后验的均值和协方差，归一化常数可以最后统一算，很多时候甚至不需要算。

注意：信息向量 η 不是状态的均值。均值要从 μ = Λ⁻¹η 反解出来。这一点特别容易搞混，我在第一次读论文时就因为这个在代码里写错过一次，症状是滤波器看起来能收敛但始终有固定偏差。

2.2 信息矩阵的物理含义与 Fisher 信息的关系

信息矩阵这个名字不是随便起的。在线性高斯模型下，Λ 恰好等于状态的对数似然函数在真值处的负 Hessian，也就是 Fisher 信息矩阵。这意味着 Λ 的每个特征值都对应一个方向上"信息量"的大小，特征值越大，该方向上参数估计的方差下界越小。

这个联系带来两个实际好处。第一，你可以用信息矩阵的行列式或者迹来衡量当前估计的质量，这个量在传感器调度和自适应采样里很常用。第二，当多个独立观测源作用在同一个状态上时，总 Fisher 信息是各源 Fisher 信息之和，这正是观测更新可加性的统计学根源。换句话说，观测更新的加法不是巧合，而是 Fisher 信息的可加性在起作用。

还有一个容易被忽略的细节：信息矩阵在加法下的可加性只对独立观测成立。如果两个传感器的噪声相关，比如它们共享同一个时钟误差或者同一个标定偏差，那么直接相加会低估后验不确定性。这时候要么显式建模互协方差，要么先把相关部分做去相关处理。我在一个多相机系统里遇到过这个问题，两个相机共用一个机械支架，振动引起的观测噪声高度相关，直接相加导致估计的置信区间比实际窄了将近一半，后来加了互协方差项才修好。

3. 观测更新推导：为什么它能变成一步加法

3.1 贝叶斯后验的指数合并

设先验是 p(x) ∝ exp(-½ x^TΛx + x^Tη)，观测模型是线性的 z = Hx + v，其中 v 服从均值为零、协方差为 R 的高斯分布。那么似然函数是

$$ p(z|x) \propto \exp\left(-\frac{1}{2}(z-Hx)^T R^{-1}(z-Hx)\right) $$

把似然的指数项展开：

$$ -\frac{1}{2}(z-Hx)^T R^{-1}(z-Hx) = -\frac{1}{2}x^T H^T R^{-1} H x + x^T H^T R^{-1} z - \frac{1}{2}z^T R^{-1} z $$

最后一项不含 x，并进常数。贝叶斯公式告诉我们后验正比于先验乘似然，两个指数函数相乘就是指数相加，把所有含 x 的项加起来：

$$ -\frac{1}{2}x^T\left(\Lambda + H^T R^{-1} H\right)x + x^T\left(\eta + H^T R^{-1} z\right) $$

对照信息形式的标准结构，直接读出后验参数：

$$ \Lambda_{post} = \Lambda + H^T R^{-1} H, \qquad \eta_{post} = \eta + H^T R^{-1} z $$

推导到这里就完了。整个过程没有出现任何矩阵求逆，唯一需要求逆的是 R，那是观测噪声协方差，维度等于观测维度，而且通常在离线阶段就能算好。

3.2 每一步为什么成立：配方与二次型合并

上面这段推导看着简单，但有两个地方值得展开说清楚，因为它们是理解后续所有性质的基础。

第一处是"指数相乘等于指数相加"。这看起来是废话，但在贝叶斯框架里它意味着先验和似然在对数域上线性叠加。如果你把对数后验写出来，会发现它是一个关于 x 的二次函数，而二次函数的系数直接就是信息矩阵。这解释了为什么信息滤波在观测更新上如此简洁：对数域上的叠加操作对应到信息参数上就是向量加法，天然没有耦合。

第二处是二次型的合并。许多人在第一次推的时候会担心，先验的二次型是以 x-μ 为中心展开的，似然的二次型是以 z 为中心展开的，两者中心不一样，合并的时候会不会出问题。答案是不会有问题，因为任意二次型都可以通过配方写成"标准中心 + 常数"的形式。常数项在归一化时被吸收，真正决定后验形状的只有二次项系数和一次项系数，而这两者正是 Λ 和 η。你可以自己拿一维情形手动展开一遍，一维情况下 P 是一个标量，信息矩阵就是它的倒数，写完你会发现后验方差等于两个方差倒数之和再取倒数，这正是并联电阻的公式，非常直观。

3.3 多源观测的顺序无关性与并行融合

假设现在有两个独立观测，观测矩阵分别是 H₁、H₂，噪声协方差 R₁、R₂，观测值 z₁、z₂。按照上面的结论，一次全融的总信息是

$$ \Lambda = \Lambda_{prior} + H_1^T R_1^{-1} H_1 + H_2^T R_2^{-1} H_2 $$

$$ \eta = \eta_{prior} + H_1^T R_1^{-1} z_1 + H_2^T R_2^{-1} z_2 $$

如果先融 z₁ 再融 z₂，中间态是 Λ + H₁ᵀR₁⁻¹H₁，再往上加第二项，结果完全相同。因为矩阵加法满足交换律和结合律，顺序完全不影响最终结果。

这个性质在工程上的价值远大于它看起来的样子。它意味着你可以为每个观测源预先算好它贡献的信息增量，存成一个固定结构，需要的时候直接加。在分布式系统里，每个观测节点只需要把 HᵀR⁻¹H 和 HR⁻¹z 这两个小矩阵通过通信链路发到融合中心，融合中心做加法即可，不需要知道其他节点的任何状态。更进一步，如果每个节点都持有一份全局信息矩阵的副本，每个节点只需要广播自己那一份增量，所有节点都能独立地得到相同的融合结果，这就是所谓的分布式信息滤波。

代价也要说清楚。虽然更新便宜了，但你要维护的是 Λ 和 η，这两个量本身不是最终想要的输出。如果你需要均值，还得做一次 Λ⁻¹η 的反解，那次求逆跑不掉。如果你需要协方差对角元来做置信区间，也要求逆。所以信息滤波省的是更新步骤里的求逆，不是所有求逆，这一点在评估整体算力时要算清楚。

4. 预测步骤推导：麻烦都堆在这里

4.1 从协方差传播公式反推

预测步骤在标准卡尔曼里很轻，一句话就写完了：先验均值往前推一步，先验协方差做一次相似变换再加过程噪声。信息形式下要麻烦得多，因为"加"在这里变成了需要求逆的操作。

设系统演化是 x' = Ax + Bu + w，w 服从均值为零、协方差为 Q 的高斯分布。在矩形式下：

$$ \mu' = A\mu + Bu, \qquad \Sigma' = A\Sigma A^T + Q $$

注意 x' 的分布仍然是高斯的，条件是从旧状态出发的线性变换加上独立高斯噪声。要求它的信息形式，就得把 Σ' 求逆：

$$ \Lambda' = (A\Sigma A^T + Q)^{-1} $$

因为 Σ = Λ⁻¹，所以

$$ \Lambda' = (A\Lambda^{-1}A^T + Q)^{-1} $$

旋转到信息视角，这一步需要做一次 Λ 的求逆得到 Σ，再做一次 AΣAᵀ+Q 的求逆得到 Λ'。两个 n×n 求逆，n 是状态维度。如果用四元数表示姿态，n 可能是十五维甚至更高，两次求逆的开销就相当可观了。

信息向量的更新是

$$ \eta' = \Lambda' \mu' = \Lambda' (A\Lambda^{-1}\eta + Bu) $$

这里又出现了一次 Λ⁻¹。也就是说，预测步骤里一共需要两次求逆，而且都必须做，没法绕开。

这是信息滤波最核心的取舍。观测更新是一步加法，预测更新是两个矩阵求逆。你的系统如果是"观测多、预测少"，比如低频率状态演化的静态传感器阵列，收益极大；如果是"预测多、观测少"，比如高频惯性递推，就要慎重。

4.2 用矩阵求逆引理把求逆维度压下来

上面那个 (AΛ⁻¹Aᵀ+Q)⁻¹ 直接求逆是 n×n 的，如果 Q 可逆，我们可以用 Woodbury 恒等式把它改写成一个不需要求 Λ⁻¹ 的形式。

Woodbury 恒等式的形式是

$$ (Q + A\Sigma A^T)^{-1} = Q^{-1} - Q^{-1}A(\Sigma^{-1} + A^T Q^{-1} A)^{-1}A^T Q^{-1} $$

代入 Σ⁻¹ = Λ 得到

$$ \Lambda' = Q^{-1} - Q^{-1}A(\Lambda + A^T Q^{-1} A)^{-1}A^T Q^{-1} $$

这个式子的好处是它只需要求一次 (Λ + AQ⁻¹A) 的逆，而且不再显式构造 Σ。如果你在预测的同时还维护着 Λ，那么这个量可以直接用现成的 Λ 计算。如果 Q 本身是分块对角或者稀疏的，Q⁻¹ 也能提前算好，整体开销进一步下降。

不过要提醒的是，Woodbury 那一步成立的默认条件是 Q 可逆。如果过程噪声在某些方向上为零，也就是系统在某些方向上是确定演化的，那么 Q 奇异，这条路走不通。这时候常见的处理是对 Q 做正则化，加上一个很小的对角项，或者改用奇异值分解的形式来规避。我自己在刚体动力学建模里遇到过这种情况，姿态的四元数部分没有过程噪声但位置部分有，处理方式是把状态拆成两个子块分别推进。

4.3 控制输入怎么接进来

控制项 Bu 的接入方式很直接，因为它只影响均值，不影响协方差。在信息向量更新里，可以先算出预测均值 μ' = Aμ + Bu，再乘上新的信息矩阵得到 η'。如果你在实现上维护的是 (μ, Σ) 和 (η, Λ) 两套量，这个步骤会更顺：用矩形式推进均值和控制，用信息形式的公式算出 Λ'，然后用 Λ'μ' 更新 η'。

这里有一个经常被忽略的细节。控制输入的误差，也就是执行器噪声，应该并进 Q 里，而不是并进 B 里。很多人会把执行器噪声写成 B 的扰动，这在推导上等价于状态相关的噪声，和标准的加性高斯白噪声假设不匹配。正确的做法是把控制噪声 u_noise 通过 B 投影到状态空间，让 Q 增加 B·Σ_u·Bᵀ 这一项。这样处理后 Q 一般会变成半正定而非正定，也就回到上一节讨论的奇异问题。

5. 两种滤波器的工程对照与选型

5.1 计算量、初始化与数值稳定性对照表

我把常见维度的开销和特性整理成一张表，供选型时参考。表中的 n 是状态维度，m 是观测维度。

关于初始化这一点值得多讲几句。标准卡尔曼滤波里，初始协方差必须正定，否则第一次更新就会出问题。但在信息滤波里，理论上 Λ = 0 是完全合法的状态，它表示"我对这个状态没有任何先验知识"，此时信息向量 η 也为零。随着观测一个个进来，Λ 从零开始逐步建起来。这个特性在分布式估计里是杀手锏：所有节点可以约定初始信息为零，各自贡献自己的观测增量，谁先上线谁先加，完全不需要协商一个共同的初始协方差。这个模式在传感器网络里几乎是标配。

数值稳定性上，信息滤波在观测精度极高的时候表现更好。原因是当 R 很小时，HᵀR⁻¹H 的量级会非常大，Λ 保持在一个很大的数值范围，浮点数的表示相对稳定。而标准卡尔曼在同样情况下，卡尔曼增益会趋近一个极限值，协方差矩阵的元素会变得极小，容易触碰浮点精度下限。我做过一个对照实验，用双精度浮点跑一个测距精度到毫米级的定位问题，卡尔曼形式在几步之后协方差出现了轻微的非对称，而信息形式跑了几百步仍然是对称的。

5.2 木村滤波为什么要维护两套参数

有一类变种叫木村滤波（Kimura Filter），它同时维护 (μ, Σ) 和 (η, Λ) 两套参数，用矩形式做预测、用信息形式做更新，再在每步之后做一次一致性同步。这样既避免了预测中的求逆，又保留了更新中的加法优势。

同步的方式很简单，从最新的一套参数算出另一套。如果当前有 (μ, Σ)，就令 Λ = Σ⁻¹、η = Λμ；如果当前有 (η, Λ)，就令 μ = Λ⁻¹η、Σ = Λ⁻¹。逻辑上无懈可击，代价是每一步都多了一次矩阵求逆，实际上把信息滤波预测步骤省下来的开销又还回去了。

那木村滤波还有意义吗？有的，但场景很窄。它的价值在于当预测步骤可以被解析简化时，比如 A 是正交矩阵或者对角矩阵，AΛ⁻¹Aᵀ 可以直接从 Λ 的对角元换算出结果，不需要做完整求逆。这时候木村滤波既得到了预测的便宜，也得到了更新的便宜。如果你的系统结构允许这种简化，值得花时间做一做，否则老老实实用一种形式就好，两套参数同步带来的复杂度和潜在的数值误差往往得不偿失。

6. 用 numpy 把整套推导跑一遍

6.1 核心类实现

下面是一个最小可运行的实现，状态维度 n 和观测维度 m 都可以动态指定。我刻意没有用任何估计库，全部手写，方便对照上面的公式。

import numpy as np class InformationFilter: def __init__(self, n, eps=1e-8): self.n = n # 用极小对角项代替严格零信息，避免求逆时奇异 self.Lambda = np.eye(n) * eps self.eta = np.zeros(n) def to_moment(self): Sigma = np.linalg.inv(self.Lambda) mu = Sigma @ self.eta return mu, Sigma def predict(self, A, Q, B=None, u=None): mu, Sigma = self.to_moment() mu_pred = A @ mu if B is not None and u is not None: mu_pred = mu_pred + B @ u Sigma_pred = A @ Sigma @ A.T + Q self.Lambda = np.linalg.inv(Sigma_pred) self.eta = self.Lambda @ mu_pred def update(self, z, H, R): Rinv = np.linalg.inv(R) self.Lambda = self.Lambda + H.T @ Rinv @ H self.eta = self.eta + H.T @ Rinv @ z def state(self): mu, _ = self.to_moment() return mu

这段代码里有两个细节值得单拎出来说。第一个是初始化时那个 eps，它对应"零信息"的近似实现。严格零信息会导致 to_moment 里的求逆失败，用一个极小值替代后，初始协方差约等于 1/eps 倍的单位矩阵，效果上就是"非常不确定"，随着观测进来会被迅速压制下去。eps 选多大有讲究，太大相当于引入了不该有的先验，太小会放大浮点误差，我一般取 1e-8 到 1e-6 之间，具体看状态的物理量纲。

第二个是 update 里完全没有出现卡尔曼增益，这直接对应上一节的推导结论，更新的全部内容就是两次加法。你可以把这段和标准卡尔曼实现放在一起对照，会发现信息形式少了整整齐齐一大块代码。

6.2 一个二维匀速运动模型的跟踪实验

我构造了一个最简单的场景来验证：一个二维平面上做匀速直线运动的点目标，状态是 [x, y, vx, vy]。过程噪声用一个小的对角矩阵，观测是每步都能测到位置，噪声协方差也是对角阵。

n = 4 dt = 0.1 A = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) Q = np.diag([1e-4, 1e-4, 1e-3, 1e-3]) H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) R = np.diag([0.01, 0.01]) inf_filter = InformationFilter(n) true_state = np.array([0.0, 0.0, 1.0, 0.5]) np.random.seed(7) for k in range(200): true_state = A @ true_state + np.random.multivariate_normal(np.zeros(n), Q) z = H @ true_state + np.random.multivariate_normal(np.zeros(2), R) inf_filter.predict(A, Q) inf_filter.update(z, H, R) print("最终估计:", inf_filter.state())

跑下来位置估计会稳定收敛到真值附近，速度估计略慢一些但也在几步之内贴合。这个场景没什么难度，重点是用它来确认代码逻辑正确，因为最简单的情况最容易暴露符号错误。我建议每个第一次实现信息滤波的人都先跑这种最简场景，别一上来就上真实的复杂模型，否则出了问题根本分不清是公式推错了还是模型设计有问题。

6.3 与标准卡尔曼滤波的一致性交叉验证

真正有价值的验证是拿同样的数据同时喂给两套实现，逐步对比状态估计。理论保证是两者结果完全一致，任何偏差都说明代码有问题。

class KalmanFilter: def __init__(self, x0, P0): self.x = x0.copy() self.P = P0.copy() def predict(self, A, Q): self.x = A @ self.x self.P = A @ self.P @ A.T + Q def update(self, z, H, R): S = H @ self.P @ H.T + R K = self.P @ H.T @ np.linalg.inv(S) self.x = self.x + K @ (z - H @ self.x) self.P = (np.eye(len(self.x)) - K @ H) @ self.P

然后用同一组模拟数据跑两条线，逐步比较输出的均值和协方差。我第一次跑这个对比的时候，发现三十步之后开始出现 1e-6 量级的偏差，当时以为是公式推错了，排查了大半天，最后发现问题出在 eps 那个初始值上，因为信息滤波的初始 Λ 是 eps 倍的单位阵，等效初始协方差是 1/eps 倍的单位阵，那个数值和卡尔曼形式里我拍的 P0 不完全相等，所以两边起点就不一样。把 P0 设成和信息滤波等效的初始协方差之后，两条轨迹贴合到小数点后十位以上。

这个坑其实挺典型。信息滤波的初始状态是个不容易直观对应的量，你很难像卡尔曼那样直接说"初始位置的不确定度是 1 米"，因为信息矩阵的对角元是世界坐标系的精度，而不是某个具体物理量的方差。工程上一般会反过来做，先想清楚初始协方差 P0 是多少，再取 Λ0 = P0⁻¹ 作为起点，这样思路更顺。

7. 实操里最容易踩的几个坑

7.1 信息矩阵奇异与零信息初始化

第一个坑前面提过，这里展开讲一下处理策略。严格零信息初始化在理论上合法，但在实现上会导致求逆失败，尤其是第一次预测的时候。三种常见的处理方式：一是加极小正则项，简单粗暴但会引入微小的先验偏差；二是第一次预测跳过，直接从第一个观测开始积累；三是用一个独立的、极低置信度的先验，比如把状态定义在一个很大的球形区域内，取协方差为单位阵乘以一个大数。

我自己的偏好是第二种和第三种结合。用一个不太大的先验协方差起步，同时在第一次预测前不做求逆，直接跳过把第一次的 Λ 定为初始值。这样可以避免那个极小正则项随迭代不断被放大。有一次我在一个长时间运行的系统中用了正则项，跑了几个小时之后发现估计开始慢慢偏移，追踪下来是因为那个 eps 在信息矩阵里始终占着一份"虚假"的确定性，随着观测的贡献被反复累加，这份虚假先验的影响在特定方向上被放大了，最终表现成了缓慢漂移。

提示：如果你的系统需要长时间不间断运行，尽量避免用正则项处理奇异，改用显式的初始化流程，把先验这件事说清楚。

7.2 观测精度极高时的数值问题

第二个坑是关于观测噪声 R 非常小的情况。举个数，如果测距精度到毫米级，那么 R 的对角元大概在 1e-6 量级，R⁻¹ 的对角元在 1e6 量级。这时 HᵀR⁻¹H 的每个元素都会非常大，和过程中累积的其他信息项相加时，小的那部分会被浮点精度吃掉。

这个现象在短期看不出来，但长时间运行之后会导致两个症状：一是数值上本该对称的信息矩阵出现轻微不对称，因为加法的顺序不同引入了舍入误差；二是本该正定的矩阵出现微小的负特征值，下一次求逆就会报错或者得到荒谬的结果。

对付这个问题的标准做法是做对称化，每步更新之后显式地把 Λ 换成 (Λ+Λ)/2，把不对称的部分抹平。如果条件允许，还可以把观测噪声协方差的对角元向上抬一点点，比如从 1e-6 抬到 1e-5，用一点点统计上的保守换取数值上的稳定性。我做过对比，抬掉半个数量级对最终估计精度的影响在可接受范围内，但引起的数值崩溃风险大幅下降。

7.3 常见问题速查表

我把实际项目中遇到的高频问题和对应处理整理如下，方便快速对照排查。

这张表里最难排查的是最后一项。理论上 Λ 是若干半正定矩阵之和，不可能是负定的，但在数值误差下确实会出现微小的负特征值。发现这个症状时不要急着改算法，先确认是不是 R 过小导致的病态，如果是，从噪声标定入手比从数值技巧入手更靠谱。

另外补充一个我遇到过的特殊情况。有一次系统需要处理来自不同时间戳的观测，旧观测要延迟才能处理，这时候如果直接按到达顺序更新，会导致信息矩阵的加法顺序和时间顺序不一致。在标准卡尔曼里这会引起混淆，因为后一个观测要先解算前一个的状态。但在信息滤波里反而更简单，只要每个观测的信息增量按它自己的时刻计算，然后统一加到当前信息矩阵里就行，不需要维护任何中间态。这个特性是异步传感器融合里非常有价值的，如果你正在做多传感器的时间对齐，信息形式会替你省掉很多麻烦。

最后分享一个我在实际做传感器标定时用的技巧。当你需要在线估计一个静止的标定参数，比如两个传感器之间的外参，可以用信息滤波在参数空间上做增量更新，每个新观测贡献一份信息增量，等系统运行一段时间后，信息矩阵的逆就是标定参数的协方差。这个做法最大的好处是随时可以看到当前的信息量，当某个方向的信息量已经饱和时就可以停止采集数据了，不用像批量最小二乘那样反复重算整个批次的解。

结尾

我从第一版能跑通的信息滤波实现到现在稳定的生产版本，前后大概迭代了三四次，每次都是在真实系统里暴露出问题之后才回来改。回头看，最难的地方不是公式推导，而是那些从公式里看不出来的东西，比如零信息初始化的具体实现、极小正则项带来的长期漂移、高精度观测下的数值崩溃。这些经验没有一篇论文会写，只能靠自己在一遍遍的调试里攒出来。如果你正准备在自己的项目里引入信息滤波，我的建议是先在一维或者二维的玩具模型上把整个流程跑通，然后拿一个真实的、有明确物理量的系统去验证，最后再考虑分布式和异步的场景。顺序反过来，大概率会在某个环节卡住找不到原因。