一、背景

自动驾驶、SLAM机器人的各类传感器均存在噪声：IMU存在漂移、GPS数据跳动、激光雷达存在测量噪声、轮速计会出现打滑。 传感器原始读数不能直接当作系统真实状态。算法目标：从带噪声的观测数据中，解算出机器人/车辆真实的位置、姿态、速度、IMU零偏。

基础概念回顾

1. KF（卡尔曼滤波）：仅支持线性高斯系统
2. EKF（扩展卡尔曼滤波）：对非线性做单次一阶泰勒线性化，递推式滤波器，只保留当前状态；工程主流使用改进版 ESKF误差状态卡尔曼滤波（本书第3章）
3. 图优化（批量最小二乘/因子图）：保存一段完整历史状态，多次迭代重新线性化，做整体求解；常用工具：ceres‑solvers、g2o；代表工程：LIO‑SAM、FAST‑LIO2

---

二、卡尔曼滤波 KF（线性卡尔曼滤波）

前提条件

系统运动方程、观测方程全部为线性，噪声服从高斯分布。

真实应用案例

1. 仿真教学实验：理想无旋转场景的定位仿真；
2. 老旧简单设备：一维/二维简单导航，匀速物体跟踪；
3. 理论底层：作为EKF、ESKF的理论基础。

✅适用场景

运动模型、观测模型严格线性，噪声为高斯白噪声。 例：物体仅做平面直线运动，无旋转，直接观测位置。

❌不适用场景

机器人、自动驾驶小车。车辆旋转、IMU积分、激光观测都是非线性，实体项目几乎不直接使用原始KF。

算法作用

1. 预测：根据上一时刻状态 + 运动输入（轮速、IMU），预测当前时刻车辆状态；
2. 更新：接收传感器观测（GPS），修正预测结果，输出最优估计，通过协方差矩阵P维护估计结果的不确定性。

通俗理解：预测一步，观测修正一步，递推输出状态。

局限

仅能处理线性模型；现实机器人系统大多是非线性。

选型条件

仅用于仿真、教学、纯线性模型；实体机器人项目直接跳过。

---

三、扩展卡尔曼滤波 EKF（工程实际使用ESKF）

核心特性

递推在线运行；每收到一组数据就输出当前状态；历史信息压缩存入协方差矩阵，不保存全部历史位姿；仅执行一次线性化，无迭代。

ESKF误差状态卡尔曼：EKF工程改进版本，规避旋转带来的奇异问题，车载惯性导航主流方案。

真实工程案例

1. IMU‑GPS组合导航：车载/无人机，IMU高频积分做预测，GPS低频到达做观测更新，输出位姿速度，同时估计IMU零偏；
2. 早期SLAM算法：早期ORB‑SLAM1的EKF版本、早期机器人定位模块；
3. 小车项目（Jetson+STM32编码器+IMU）：STM32轮速编码器做运动输入，IMU提供角速度加速度，GPS作为观测，ESKF输出小车实时定位。

✅适用场景

1. 需要低延迟实时输出定位，硬件算力有限（嵌入式、Jetson、车载MCU）；
2. 只关心当前时刻状态，不需要修正久远的历史轨迹；
3. 无回环检测，不需要对历史轨迹做大规模修正；
4. 系统非线性程度不剧烈，初始估计误差不大。

❌不适用场景

1. 初始位姿误差很大，单次线性化容易发散；
2. 需要回环检测，修正大量历史帧轨迹；
3. 需要高精度离线后处理。

算法作用

针对车辆运动、旋转的非线性，在当前估计点做泰勒展开近似为线性，复用KF的预测‑更新框架。

1. 预测：IMU积分预测车辆位置、姿态、速度、IMU陀螺/加速度计零偏；
2. 更新：GPS、激光、视觉特征点作为观测，修正预测结果，抑制IMU漂移。

缺点

只做一次线性化；当初始估计误差大，线性化点不准，结果会跑偏甚至发散。

选型条件

需要实时在线定位、低延迟、算力有限，不需要回环修正历史轨迹 → 选择ESKF。

自动驾驶车辆、机器人小车、无人机飞控实时定位优先ESKF，尽量不直接使用原生EKF。

---

四、图优化 / 批量最小二乘（因子图）

核心特性

存储一段时间全部时刻状态；IMU、轮速、GPS、激光、视觉作为约束因子；多次迭代更新线性化点，整体求解全部位姿。

真实工程案例

1. 激光SLAM后端：LIO‑SAM、FAST‑LIO2、LOAM系列；激光点云、IMU预积分作为约束，回环触发后整体优化历史轨迹，消除里程计累计漂移；
2. 视觉SLAM：ORB‑SLAM3后端，BA光束平差；
3. 自动驾驶数据集离线后处理、离线标定；
4. 多传感器外参标定：IMU‑激光雷达、IMU‑相机外参标定。

✅适用场景

1. 需要回环检测，修正历史轨迹，消除长时间累计误差；
2. 允许一定延迟，可以缓存小段历史数据；
3. 追求高精度，硬件算力充足（Jetson Nano/NX可运行，但算力压力大）；
4. 离线数据集处理、传感器标定。

❌不适用场景

1. 低端MCU，算力紧张，无法存储大量历史帧；
2. 要求极低延迟，完全不能缓存历史数据。

算法作用

1. 将每个时刻车辆位姿作为图的顶点；
2. 运动因子：IMU、轮速计提供相邻时刻运动约束；
3. 观测因子：GPS、激光、视觉提供观测约束；
4. 将全部约束写成残差，高斯‑牛顿迭代求解，得到全部时刻最优轨迹。

✅优点：多次迭代更新线性化点，精度高；支持回环修正，消除累计漂移。 ❌缺点：计算量大，需要保存全部历史状态；实时运行算力开销高于EKF。

选型条件

1. 做SLAM建图，需要回环修正历史轨迹 → 选图优化/因子图；
2. 离线数据集处理、标定，不计较实时延迟 → 选图优化；

   如果仅需要实时定位，不需要建图回环，使用图优化会带来不必要算力开销。

---

五、EKF(ESKF) 与图优化核心对比

算法	工作方式	特点	典型用途
EKF/ESKF	递推，只输出当前时刻状态，历史信息被压缩	计算量小，实时性好；无法修正历史误差	车载实时定位、IMU‑GPS组合导航，要求低延迟
图优化/因子图	批量处理，保存全部历史，所有状态一起迭代求解	精度高，支持回环修正；算力消耗大	SLAM建图、离线后处理、高精度激光融合

通俗类比

• EKF：开车的同时，实时估算自己当前的位置；
• 图优化：开完一段路程后，复盘整段行驶记录，修正整条轨迹。

---

六、选型决策表

项目需求	优先选择	备注
仿真教学，系统严格线性	KF	实体项目不要使用
小车仅做实时定位，无需建图回环；IMU+轮速编码器+GPS；Jetson嵌入式，要求低延迟输出位姿	ESKF（EKF改进版）	可最适配Jetson+STM32小车
做SLAM建图，搭载激光雷达，需要回环消除漂移，Jetson算力尚可	图优化（因子图）	FAST‑LIO2、LIO‑SAM均为此方案
录制数据集，离线处理、标定传感器外参	图优化	不计延迟，追求精度

七、工程主流组合方案

现在自动驾驶常用：ESKF + 图优化组合 前端ESKF保证低延迟实时输出定位；后台子线程运行因子图优化；回环检测触发后，将优化结果反馈给滤波器，兼顾实时性与回环修正能力。

八、针对硬件实操（Jetson Nano + STM32 + IMU）

1. 只实现小车定位：ESKF误差状态卡尔曼滤波，不必直接上手图优化。STM32上报轮速编码器、IMU数据送入ESKF，输出小车位姿。
2. 后续增加激光雷达做SLAM建图：再学习图优化，完成回环与轨迹优化。

⚠️避坑提醒：不要一开始就直接使用图优化，会占满Jetson Nano CPU，虚拟机编译也会压力巨大。定位优先ESKF，建图再用图优化。