【SLAM】从卡尔曼滤波到非线性优化：状态估计的演进与选择
1. 状态估计基础：从传感器数据到概率模型
当你第一次拆开扫地机器人的外壳，会看到激光雷达不断旋转扫描周围环境，轮子上的编码器记录着转动圈数，IMU（惯性测量单元）则感知着机器的加速度和角速度。这些传感器就像机器人的"感官"，但它们提供的原始数据充满噪声和不确定性。状态估计的核心任务，就是把这些零散的、带噪声的观测数据，融合成对系统内部状态（如位置、速度）的可靠估计。
想象你在雾天开车，挡风玻璃上雨滴模糊了视线（观测噪声），方向盘存在轻微虚位（控制噪声）。此时你需要结合车速表读数、偶尔瞥见的道路标志，以及记忆中的路线，在脑海中构建当前位置的"最可能"地图——这正是SLAM系统状态估计的日常挑战。
传统卡尔曼滤波建立在线性系统假设上：
# 典型线性系统模型 x_k = F * x_{k-1} + B * u_k + w_k # 状态方程 z_k = H * x_k + v_k # 观测方程其中w_k和v_k代表过程噪声和观测噪声，通常假设为高斯白噪声。这种表示法的强大之处在于，它将物理系统的动态变化转化为矩阵运算，使得计算机可以高效处理。不过在实际SLAM中，我们更常面对的是非线性系统...
2. 卡尔曼滤波：线性世界的优雅解法
1960年，Rudolf Kalman发表的那篇著名论文，最初是为阿波罗登月计划的导航系统设计的。卡尔曼滤波的魅力在于它像一位理性的赌徒：每次下注都基于当前筹码（状态估计）和最新牌面（观测数据），通过概率计算做出最优决策。
让我们用无人机悬停的例子具体说明。假设无人机需要维持在10米高度：
- 预测阶段：根据上一秒高度9.8米和上升速度0.3m/s，预测当前高度应为10.1米
- 更新阶段：气压计测得实际高度10.2米（存在±0.5米误差）
- 数据融合：卡尔曼增益会计算应该更相信预测（可能更稳定）还是观测（可能反映突发气流）
卡尔曼滤波的数学之美体现在其五步递归公式：
预测： x̂_k|k-1 = F_k * x̂_k-1|k-1 + B_k * u_k P_k|k-1 = F_k * P_k-1|k-1 * F_k^T + Q_k 更新： K_k = P_k|k-1 * H_k^T * (H_k * P_k|k-1 * H_k^T + R_k)^-1 x̂_k|k = x̂_k|k-1 + K_k * (z_k - H_k * x̂_k|k-1) P_k|k = (I - K_k * H_k) * P_k|k-1其中协方差矩阵P就像"置信度分数"，决定了系统对预测和观测的信任权重。我在早期项目中使用卡尔曼滤波做无人机定位时，曾因Q（过程噪声协方差）设置不当导致滤波器过度信任运动模型，结果撞上了突然出现的障碍物——这个教训让我深刻理解到噪声建模的重要性。
3. 当世界变得非线性：EKF的妥协与局限
现实中的SLAM问题本质是非线性的：相机的投影模型、旋转运动、传感器特性等都存在非线性。扩展卡尔曼滤波(EKF)的解决思路是在工作点处进行一阶泰勒展开，用线性近似处理非线性问题。
以移动机器人里程计为例：
# 非线性运动模型 def motion_model(x, u): theta = x[2] return np.array([ x[0] + u[0]*np.cos(theta), x[1] + u[0]*np.sin(theta), x[2] + u[1] ]) # EKF处理需要计算雅可比矩阵 F_jacobian = np.array([ [1, 0, -u[0]*np.sin(theta)], [0, 1, u[0]*np.cos(theta)], [0, 0, 1] ])我曾用EKF实现过视觉惯性里程计(VIO)，当机器人快速转弯时，线性近似误差会导致明显的位姿漂移。更棘手的是，EKF需要手动推导雅可比矩阵——对于复杂的传感器模型，这个过程既容易出错又难以维护。某次项目中因为雅可比矩阵一个符号错误，导致定位系统在30分钟后完全发散，这个bug花了我们整整两周才追踪到。
4. 非线性优化的崛起：因子图与图优化
现代SLAM系统如Google的Cartographer、MIT的Kimera都采用基于图优化的方法，这背后是非线性最小二乘的数学框架。与卡尔曼滤波的"逐步修正"不同，图优化会构建所有观测的全局约束网络。
因子图(factor graph)是一种直观的表示方式：
- 变量节点：需要估计的状态（位姿、路标点）
- 因子节点：各种约束（IMU预积分、视觉重投影、闭环检测）
// 典型g2o问题构建 g2o::SparseOptimizer optimizer; // 添加顶点（待优化变量） g2o::VertexSE3* v1 = new g2o::VertexSE3(); v1->setId(0); optimizer.addVertex(v1); // 添加边（约束） g2o::EdgeSE3* e1 = new g2o::EdgeSE3(); e1->setVertex(0, v1); e1->setMeasurement(T_meas); optimizer.addEdge(e1);在无人机集群定位项目中，我们比较过EKF与g2o优化的效果：当处理20架无人机相互观测数据时，EKF因为要维护巨大的状态协方差矩阵（约4000x4000），计算耗时呈指数增长；而基于稀疏性的图优化，通过利用QR分解或Cholesky分解等技巧，仍能保持实时性。
5. 技术选型指南：从理论到工程实践
选择状态估计方法时，需要权衡多个维度：
| 维度 | 卡尔曼滤波家族 | 非线性优化方法 |
|---|---|---|
| 计算效率 | O(n^2) | O(n)~O(n^2)（稀疏性） |
| 内存消耗 | 需存储完整协方差矩阵 | 只需存储非零元素 |
| 线性假设 | 需要（EKF需局部线性化） | 无限制 |
| 全局一致性 | 难以保证 | 通过回环检测自然实现 |
| 实现复杂度 | 较低 | 较高 |
| 对初始值敏感性 | 敏感 | 相对鲁棒 |
在资源受限的嵌入式平台（如STM32），我仍会选用卡尔曼滤波：曾为农业无人机设计的高度控制系统，在仅有1MB RAM的条件下，通过精心设计的降维EKF实现了厘米级精度。而在服务器端的多机器人建图系统中，我们采用iSAM2增量平滑算法，能实时处理包含数百万变量的优化问题。
卡尔曼滤波就像瑞士军刀——在简单场景下可靠易用；非线性优化则像专业手术刀，需要更多技能但能处理复杂病例。理解这两种工具的数学本质和工程特性，才能为你的SLAM系统选择最佳状态估计方案。