【免费下载链接】INSLIB
Open Source Inertial Navigation Library
关键词：INSLIB 惯性导航、误差状态卡尔曼滤波（ESKF）、15 状态/18 状态导航滤波器、GNSS/INS 组合导航、Bierman UDU 平方根滤波
INSLIB是一个开源的 C 语言三维导航状态估计库，其核心 src/ins.c 实现了一个15/18 状态误差状态卡尔曼滤波（Error-State Kalman Filter, ESKF），融合 IMU、GNSS、磁力计、气压计等传感器，输出位置、速度、姿态与传感器偏置。本文将用尽量少的代码，带你完整理解这套数学原理：状态向量怎么定义、预测与更新各做什么、为什么用平方根形式、以及 18 状态模式比 15 状态多了什么。
一、为什么要用"误差状态"滤波？
传统卡尔曼滤波直接对位置、姿态等真实状态建模，但姿态（四元数/旋转矩阵）是非线性的，直接滤波很复杂。误差状态卡尔曼滤波（ESKF）的思路更聪明：
| 概念 | 说明 |
|---|---|
| 标称状态（Nominal） | 由 IMU 单独"推算"（strapdown）得到的位置、速度、姿态，是"最优猜测" |
| 误差状态（Error-State） | 标称值相对真实值的偏差 dx，始终在零附近的小量 |
| 线性化 | 滤波器只对"小误差"做线性卡尔曼滤波，非线性全部留在标称推算里 |
这样做的好处：误差永远是小量 → 线性近似始终有效；姿态修正用"小角度 rpy"表示，数学上干净。INSLIB 的标称状态在本地 NED 坐标系中用四元数qbn推算（见 src/ins.c 的ins_strapdown()），而滤波器本体只处理 15 或 18 维误差。
二、15/18 状态向量长什么样？
状态向量的定义写在 src/ins.h 的文件头注释中，索引用宏定义在 src/ins.h：
| 索引 | 状态 | 单位 | 含义 |
|---|---|---|---|
| 0–2 | 位置误差 | m | NED 坐标系下的位置偏差 |
| 3–5 | 速度误差 | m/s | NED 坐标系下的速度偏差 |
| 6–8 | 姿态误差 | rad | 小角度 roll/pitch/yaw |
| 9–11 | 加速度计偏置 | m/s² | 机体坐标系 |
| 12–14 | 陀螺仪偏置 | rad/s | 机体坐标系 |
| 15–17 | 磁力计偏置 | µT | 仅 18 状态模式启用 |
- 15 状态模式（
INS_UNKNOWNS = 15）：基础模式，磁力计只用于"航向纠偏"，不估计其偏置。 - 18 状态模式（
INS_UNKNOWNS_MAG = 18）：通过estimate_mag_bias选项打开，把磁力计硬铁偏置也纳入随机游走估计，适合磁力计偏置会缓慢漂移的场景。
一个关键设计细节：协方差矩阵不存 15×15（或 18×18）整阵，而是存 UDU 分解（单位上三角阵U+ 对角向量d），见 src/ins.h。原因见第五节。
三、预测步（Predict）：误差如何随时间演化
每一帧 IMU 数据到来时，INSLIB 做两件事（ins_update()= 预测 + 修正，见 src/ins.c）：
1. 标称状态推算（strapdown）
用去偏置后的陀螺仪更新姿态四元数，用去偏置后的加速度计积分出速度与位置，并加入重力、科氏力、地球自转等修正（src/ins.c）。这就是"惯性推算"，它会随时间不断漂移。
2. 误差状态预测：Φ 矩阵
误差协方差按离散卡尔曼预测方程传播：
P⁻ = Φ · P⁺ · Φᵀ + G · diag(Q) · Gᵀ其中 Φ（状态转移雅可比，见 src/ins.c 的ins_compute_Phi()）捕捉状态之间的物理耦合：
- 位置 ← 速度：速度偏差会随时间积累成位置偏差，对应 Φ 中 POS–VEL 块的
dt； - 速度 ← 姿态：姿态偏差会让加速度投影方向出错，耦合项含比力分量
fₙ × dt； - 速度 ← 加速度计偏置：偏置直接积分进速度，耦合项为
-R_b_to_n · dt； - 姿态 ← 陀螺仪偏置：同理，
-R_b_to_n · dt； - 18 状态模式的磁偏置块：纯随机游走，Φ 就是单位阵。
过程噪声则用"噪声输入"形式注入：12 路 IMU 噪声（加速度白噪声、陀螺白噪声、加速度计偏置随机游走、陀螺偏置随机游走）经噪声配置矩阵 G 映射到误差状态（src/ins.c），每列方差为PSD × dt。
四、更新步（Update）：量测如何拉回真实
当 GNSS 定位、GNSS 速度、磁力计、气压高度、零速、局部位置等量测到达时，统一走同一条"误差状态融合"流水线（src/ins.c）：
- 构造残差（innovation）：
z = h(标称状态) − z_量测。因为误差状态 dx 定义在零附近，残差近似满足z ≈ H·dx + 噪声，其中 H 是 1×15（或 3×15）的观测矩阵。例如 GNSS 位置量测的 H 就是[I₃, 0, 0, 0, 0]。 - 卡方（χ²）门控：检验
zᵀS⁻¹z > χ²阈值是否成立（阈值由配置chi2_reject_alpha映射，见 src/ins.c）。超标残差会被降权而非直接丢弃——多路径、跳变等 GNSS 故障因此不会污染解算。 - Bierman UDU 更新：对 (U, d) 逐行做平方根形式卡尔曼更新，得到误差修正量 dx。
- 回写标称状态：
标称状态 ← 标称状态 − dx，姿态用小角度修正到四元数，然后误差状态清零，协方差同步刷新（src/ins.c 的ins_apply_correction()）。
由于每次融合后误差都"重置归零"，滤波器只需要维护一个恒定的小维度——这正是 ESKF 高效的原因。
上图为 INSLIB 处理的实测轨迹输出，可导入 Google Earth 查看，用于直观验证滤波结果。
五、为什么用 Bierman/Thornton UDU 平方根滤波？
普通卡尔曼滤波直接维护协方差阵 P，长时间运行后 P 可能因浮点舍入失去正定性，导致滤波崩溃。INSLIB 采用 KFCore 子模块（KFCore/，含kalman_udu.c）的UDU 分解：
P = U · diag(d) · Uᵀ- U 是单位上三角阵，d 是对角向量；
- 预测与更新都在分解形式上进行，数值上保证协方差半正定；
- 单精度 float 下等效精度更高，适合 MCU 等资源受限平台。
这也是 readme.md 中列出的核心特性之一："Robust UDU/Bierman-Thornton Kalman Filter routines for numerically robust square-root filtering"。
INSLIB 自带的标定工具（calib），用于获取加速度计/陀螺仪偏置与安装角——良好的标定数据能显著提升滤波收敛质量。
六、一个完整的滤波周期（小结）
把整条链路串起来，每个 IMU 周期（典型 100–1000 Hz）执行：
- Strapdown：标称姿态/速度/位置前推一帧；
- Predict：Φ 矩阵 + 过程噪声传播误差协方差（UDU 形式）；
- Update：若有量测（GNSS/磁力计/气压/局部位置…），构造残差 → χ² 门控 → Bierman 更新 → 修正回写标称状态 → 误差清零；
- 输出：位置（lat/lon/高度，双精度锚点增量记账）、速度、四元数/RPY、传感器偏置及其标准差。
延迟量测（GNSS 晚到最多 500 ms）由历史环形缓冲（20 ms 网格）锚定回正确时刻融合——因为误差在几百毫秒内近似不变（Φ ≈ I），该机制代价极低（src/ins.h）。
七、动手验证：从数据集到结果
想亲眼看到这套算法工作？仓库自带大量数据集与工具：
- 内置数据集：datasets/ 下的
tunnel（GNSS 隧道丢失）、kfgins（与 KF-GINS 对标）、simulated（带真实偏置真值的仿真）、crazyflie（微型无人机）等，每套都含config.yaml+imu.csv/gnss.csv/ref.csv； - 回放与后处理：tools/replay.py、tools/ins_solution.py、tools/ins_rts.py（RTS 平滑）；
- 图形界面：tools/inspostgui.py，可加载 CSV 数据实时观察滤波结果；
- 数学背景与滤波设计详见官方手册 doc/INSLIB_manual.pdf。
例如运行回放处理隧道数据集（GNSS 中断后由惯性推算支撑）：
python3 tools/replay.py datasets/tunnel/config.yaml处理完成后可用ins_solution.py与ref.csv真值对比 RMS 误差，验证 15/18 状态滤波的收敛行为。
八、总结
| 要点 | 一句话概括 |
|---|---|
| 状态维度 | 15 状态基础 + 可选 3 维磁偏置（18 状态） |
| 核心思想 | 标称状态惯性推算，误差状态线性卡尔曼滤波 |
| 预测 | Φ 雅可比捕捉物理耦合，噪声经 G 矩阵注入 |
| 更新 | 残差 → χ² 门控 → Bierman UDU 平方根更新 → 回写清零 |
| 数值鲁棒 | UDU 分解保证协方差半正定，float 精度即可运行 |
| 工程化 | 零堆分配、延迟量测历史缓冲、丰富诊断计数 |
INSLIB 把经典的 ESKF 理论（可参考 Grewal《Kalman Filtering: Theory and Practice》）落成了一个约 6 千行、单精度、可移植到 MCU 的 C 实现，是学习惯性导航组合滤波原理与工程实践的理想范本。核心源码入口：src/ins.h（接口与状态定义）、src/ins.c（滤波实现）。
【免费下载链接】INSLIB
Open Source Inertial Navigation Library
创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考
