专业知识的应用02.卡尔曼滤波我们怎么用

前言

卡尔曼滤波（Kalman Filter）在控制、导航、目标跟踪、传感器融合等很多领域都会用到。

它解决的是「在带有噪声的观测和带有噪声的系统方程下，如何得到最优状态估计」这一类问题。

不同领域的状态、观测、噪声模型各不相同，但「预测 + 更新」的框架是相通的。

本篇不推导卡尔曼滤波的公式，也不写成一节通用教材，而是只讲在本项目里卡尔曼是怎么用的：我们选了什么状态、什么观测，预测和更新在代码里对应哪一块，噪声从哪里来。

公式的严格推导和误差方程的来源，请直接参考牛小骥老师的《惯性导航原理与算法设计》等教材。

卡尔曼在本项目里的角色

在本项目中，卡尔曼采用的是误差状态形式（Error State Kalman Filter，ESKF）。

它的角色可以一句话概括：把 GNSS 的位置观测和 IMU 惯导解算得到的名义状态融合起来，估计出当前名义状态的误差（位置、速度、姿态、陀螺零偏、加速度计零偏），再用这些误差去修正名义状态。

这样，对外输出的位置和姿态就是「惯导递推 + GNSS 校正」后的结果，比纯惯导更稳定，比纯 GNSS 更平滑、且能给出姿态和零偏。

因此，卡尔曼在本项目里估计的不是位置、速度、姿态本身，而是这些量相对于惯导解算结果的误差。

惯导负责用 IMU 数据不断递推，得到名义状态；卡尔曼负责在每次 GNSS 观测到来时，用「GNSS 位置 − 当前名义位置」作为观测残差，更新误差状态的估计；

然后把误差注入到名义状态上，并重置误差状态，下一段递推又从「校正后的名义状态」开始。

这样惯导的初值始终是「组合后的最优值」，不会一路漂下去。

这一套「名义状态 + 误差状态」的闭环，在下一篇讲整体数据流时会再展开；这里先聚焦卡尔曼本身：状态是什么、预测做什么、更新做什么。

下图给出了本项目中 IMU、惯导解算、误差状态卡尔曼滤波以及名义状态之间的整体数据流。

惯导负责高频递推，卡尔曼负责在 GNSS 到来时估计误差并修正名义状态，二者形成闭环。

实际用到的概念（点到为止）

状态：误差状态与 15 维定义

卡尔曼估计的是误差状态，在代码里对应 state/state.h 中的 GIState 结构体：

• posDelta：位置误差（3 维，NED 坐标系，单位：米）
• velDelta：速度误差（3 维，NED，单位：m/s）
• attDelta：姿态误差（3 维，在导航系下采用左扰动时的旋转向量，单位：弧度）
• bg：陀螺零偏误差（3 维）
• ba：加速度计零偏误差（3 维）

共 15 维。

为什么用误差状态而不是直接估计位置、速度、姿态（全状态）？

主要有三点：一是数值上更稳，姿态用四元数时存在约束，直接在全状态空间里做协方差更新容易破坏归一化；

二是和惯导的闭环方式天然匹配——惯导是积分系统，我们每次用「当前名义状态 + 误差状态估计」去修正名义状态，然后把误差状态置零，下一段递推又从新的名义状态开始，实现起来就是一次「误差注入 + reset」，不需要在全状态空间里做复杂的约束处理。

三是减少非线性误差的影响,因为使用误差状态进行建模，可以让系统更加符合线性化假设，减少非线性的影响。

误差状态的协方差矩阵是 15×15，在滤波器里用 P_ 表示；初始值来自配置文件中的 initial_covariance（见下文「观测噪声与配置」）。

预测：误差状态传播与协方差传播

预测对应「在没有 GNSS 观测的时候，误差状态和它的不确定性如何随时间变化」。

在代码里就是 filter/gi_loose_filter.cpp 中的 predict(insState, imuData, dt)。

• 误差状态传播：误差状态满足一个线性化的微分方程，离散化后写成 δx_k = Φ·δx_{k-1}。其中 Φ（Phi） 是 15×15 的状态转移矩阵，由当前惯导状态和 IMU 数据、地球模型（子午圈/卯酉圈半径、地球自转等）决定，在代码里由 buildPhi(insState, imuData, dt) 构造；项目里用一阶近似 Φ ≈ I + F·dt，F 是误差方程的系数矩阵。也就是说，预测时用当前的名义状态（位置、速度、姿态）和 IMU 数据，把上一时刻的误差状态按 Φ 传播到当前时刻。

• 协方差传播：误差状态的不确定性用协方差矩阵 P 表示。预测时，P 的更新为 P_k|k-1 = Φ·P_{k-1}·Φ^T + G·Q·G^T。第一项是状态传播带来的不确定性变化，第二项是过程噪声带来的不确定性增加。G 是 15×12 的过程噪声驱动矩阵，在代码里由 buildG(insState) 构造，把 12 维的过程噪声（陀螺/加表白噪声、零偏驱动噪声）映射到 15 维误差状态空间；Q 是 12×12 的过程噪声协方差矩阵，由 IMU 的 ARW、VRW、零偏稳定性等参数在配置里给出，通过 GinsConfig 构建。 所以预测一步之后，P 会变大——表示在没有 GNSS 观测时，我们对误差的把握会随时间变差。

预测步骤完全由 IMU 驱动，在没有 GNSS 的情况下，误差状态和协方差会持续传播，P 随时间增大，表示系统不确定性累积。

更新：GNSS 位置观测与观测方程

更新发生在有 GNSS 观测的时候。

我们把「当前名义状态对应的位置（必要时加上天线杆臂）」和「GNSS 给出的位置」做差，得到观测残差；在松耦合里，这个残差就是在 NED 下的位置误差的观测。

观测方程写为 z = H·δx + v：z 是 3 维观测残差（NED 下的位置差），H 是 3×15 的观测矩阵，本项目里只观测位置，所以 H = [I_{3×3} 0 0 0 0]；v 是观测噪声，协方差为 R（3×3），表示 GNSS 位置的 uncertainty。

在代码里，更新对应 GILooseFilter::update(dz)。

传入的 dz 就是 NED 下的位置观测残差（在 pose_estimator 里由 GNSS 位置与当前名义位置做差、并转换到 NED 得到）。

更新步骤是标准的卡尔曼形式：先算新息协方差 S = H·P·H^T + R，再算增益 K = P·H^T·S，然后用 K 和观测残差修正误差状态，并更新协方差 P。

项目里协方差更新采用 Joseph 形式（P = (I−KH)·P·(I−KH)^T + K·R·K^T），以保证数值上的对称正定，避免长期运行后 P 发散。

需要注意的是，卡尔曼滤波器并不直接输出最终位姿，而是输出对当前惯导解算结果的修正量；只有在误差注入完成后，名义状态才是对外可用的导航结果。

观测噪声与配置

R（观测噪声协方差）和 Q（过程噪声协方差）、P（初始误差协方差）都来自配置文件，而不是在代码里写死。在 config/config.yaml 里可以看到：

• gnss.position：表示 GNSS 在北、东、地三个方向上的位置标准差（米），用于构造 R；
• imu 下的 arw、vrw、gbstd、abstd、corrtime 等：用于构造 Q；
• initial_covariance：各误差状态分量的初始标准差，用于构造初始 P。

这些矩阵在程序启动时由 GinsConfig 从 YAML 读入并组装成 FilterModel（Q、R、P），再传给 GILooseFilter 的构造函数。

因此，观测噪声、过程噪声、初始不确定性都是事先配置好的，对应第一篇里说的「外参与传感器精度事先配置」的原则。

和课本的界限

本篇没有推导状态转移矩阵 Φ 的每一项（例如位置误差如何受速度、姿态、零偏影响），也没有推导观测矩阵 H 的由来。

这些内容在组合导航教材里都有完整推导，例如牛小骥老师的《惯性导航原理与算法设计》。

我们只说明了：在本项目里，Φ 由 buildPhi 根据当前惯导状态和 IMU 数据、地球模型构造，G 由 buildG 构造；预测对应 predict() 里的 Φ、G、Q、P 的更新；更新对应 update(dz) 里的 H、R、增益 K 和 Joseph 形式的 P 更新。

读者若要「知其所以然」，直接查教材中误差方程与离散化即可。

同一套思想在其他领域的可迁移性

卡尔曼的「预测 + 观测更新」框架在本项目里用来融合 IMU 与 GNSS；

在目标跟踪里可以用来融合运动模型与雷达/视觉观测；在多传感器融合里可以用来融合多种位姿或距离观测。

状态和观测的具体定义不同，但「用动力学模型做预测、用观测方程做更新、用协方差表示不确定性」的思路是共通的。

所以把「卡尔曼在本项目里怎么用」搞清楚，有助于在其他领域快速套用同一套思维。

个人对于卡尔曼滤波的理解

从严肃的角度讲，卡尔曼滤波是线性高斯噪声假设条件下的贝叶斯滤波的递推形式的解析解。

但是，我们可以在直观上，将一个完整的卡尔曼滤波过程当成一次， 经验+情报的分析过程。

预测，就是我们根据经验得到的，事情本身的大概的演进方向；更新就是我们利用情报对猜测的一次小范围的调整。

通过这两个过程，我们就能在迷雾中得到事务最有可能的真相。

代码对应小结

• 状态定义：state/state.h 中的 GIState（posDelta、velDelta、attDelta、bg、ba），以及滤波器内部的 15×15 协方差 P_。
• 预测：filter/gi_loose_filter.cpp 中的 predict(insState, imuData, dt)：调用 buildPhi、buildG，更新误差状态与 P = Φ·P·Φ^T + G·Q·G^T。
• 更新：GILooseFilter::update(dz)：构造 H = [I 0 0 0 0]，算 S、K，用 Joseph 形式更新 P，用 K·dz 修正误差状态（posDelta、velDelta、attDelta、bg、ba）。
• 配置：filter/filter_model.h 中的 FilterModel（Q、R、P）、ImuNoise、GnssNoise、InitialStateNoise；filter/gins_config 从 config/config.yaml 读入并构建 Q、R、P。
• 调用关系：estimator/pose_estimator.cpp 中，propagate() 里对每个 IMU 步调用 filter_.predict(...)；有 GNSS 时在 gnssUpdate() 里计算 NED 位置残差 dz 并调用 filter_.update(dz)，随后 correctPose() 把误差状态注入名义状态并 filter_.reset()。

总结

1. 角色：在本项目中，卡尔曼（误差状态形式）负责把 GNSS 位置与 IMU 惯导解算结果融合，估计位置、速度、姿态、陀螺零偏、加表零偏的误差，再通过误差注入修正名义状态。
2. 状态与预测/更新：状态为 15 维误差状态（GIState）；预测用 Φ、G、Q 做误差状态与协方差传播（predict）；更新用 GNSS 位置残差和 H、R 做观测更新（update），协方差采用 Joseph 形式；Q、R、初始 P 均来自配置文件。
3. 不写什么：不推导卡尔曼公式和误差方程，只说明「我们选了什么、在代码里对应哪一块」；公式与推导见牛小骥《惯性导航原理与算法设计》等教材。同一套预测+更新的思想也可迁移到目标跟踪、多传感器融合等领域。