第四章 数学基础（三）：状态估计与概率论
好，咱们继续往下走。前面两章讲了坐标变换和传感器模型，那些都是「静态」的东西。但SLAM系统跑起来之后，你面对的是连续不断的数据流——激光扫一圈、IMU跳一下、轮子转一圈……这些数据全都有噪声。怎么从这些乱七八糟的数据里「猜」出机器人的真实状态？这就是状态估计要干的事。
我个人习惯把状态估计叫做「在不确定中找确定」。你想想看，每个传感器都在骗你，但骗的程度不一样。我们要做的，就是把所有「骗子」的证词综合起来，找出最可信的那个真相。
4.1 概率论：SLAM的底层语言
为什么SLAM离不开概率论？因为真实世界不是理想世界。我记得刚入行时，总觉得卡尔曼滤波就是个公式，套进去就能用。直到有一次在工厂调试AGV，激光雷达在金属货架间疯狂跳变，我才意识到——没有概率思维，你连传感器数据都不敢信。
几个核心概念，我挑最常用的说：
- 条件概率 P(A|B)：已知B发生，A的概率。在SLAM里，就是「已知上一帧位姿，当前帧位姿的概率」。
- 贝叶斯公式：后验 = 似然 × 先验 / 证据。说白了就是「用观测数据修正你的猜测」。
- 独立与不相关：两个传感器如果独立，联合概率可以直接相乘。但实际中……嗯，IMU和轮式里程计其实有相关性，这点后面会讲。
核心思想：SLAM本质上是一个贝叶斯推断问题。我们想求的是 P(位姿, 地图 | 观测, 控制)，也就是「给定所有传感器数据和控制指令，机器人在哪、周围长什么样」。
4.2 高斯分布：为什么它无处不在？
高斯分布，也叫正态分布。为什么SLAM里到处都是它？两个原因：
- 中心极限定理：很多独立随机变量加起来，趋近于高斯分布。传感器噪声大多符合这个规律。
- 数学性质好：高斯分布经过线性变换还是高斯分布，边缘分布和条件分布也都是高斯分布。这太方便了。
一个一维高斯分布用均值和方差就能描述。多维的呢？用均值向量和协方差矩阵。协方差矩阵的对角线是每个变量的方差，非对角线是变量之间的相关性。
我的经验：调试SLAM系统时，我经常把协方差矩阵可视化出来。如果某个维度的方差突然变大，说明传感器在那个方向上不可靠——比如激光雷达在长走廊里，沿走廊方向的定位方差会很大。看到这个，你就知道该加约束了。
高斯分布还有一个重要性质：两个高斯分布的乘积还是高斯分布（归一化后）。这就是卡尔曼滤波能递归计算的根本原因。
4.3 卡尔曼滤波基础
卡尔曼滤波，说白了就是「用上一刻的状态预测下一刻，再用观测值修正」。我刚开始学的时候，觉得五个公式背下来就行了。后来发现，理解它的物理意义比背公式重要得多。
卡尔曼滤波分两步：
- 预测步：根据运动模型，从 k-1 时刻的状态预测 k 时刻的状态。同时，不确定性（协方差）会变大。
- 更新步：拿到观测值后，计算卡尔曼增益，决定「信预测多一点还是信观测多一点」。然后更新状态和协方差。
卡尔曼增益 K 是关键。K 大，说明观测可靠，修正幅度大；K 小，说明预测可靠，修正幅度小。我曾经在项目中遇到过传感器突然失效的情况，协方差矩阵发散，K 变得异常大，导致状态估计直接崩了。后来加了异常检测，协方差超过阈值就强制重置。
注意：标准卡尔曼滤波假设系统是线性的、噪声是高斯的。如果你的系统是非线性的（比如机器人转弯），那就需要用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）。
4.4 非线性优化基础
卡尔曼滤波虽然好用，但它有个问题：它假设状态是马尔可夫的——当前状态只依赖上一时刻。但SLAM里，机器人会反复经过同一个地方（回环检测），这时候就需要把整个轨迹和地图一起优化。
这就是非线性优化的用武之地。它的核心思想是：找到一个状态，使得所有观测误差的平方和最小。
数学上，我们定义误差函数 e(x) = 观测值 - 预测值。然后最小化 e(x)^T * Σ^{-1} * e(x)，其中 Σ 是协方差矩阵，用来给不同传感器加权。
求解这个最小化问题，最常用的方法是高斯-牛顿法或LM（列文伯格-马夸尔特）法。它们的思路类似：
- 在当前状态附近，把误差函数线性化（泰勒展开）
- 求解线性最小二乘问题，得到状态增量
- 更新状态，重复直到收敛
// 伪代码：高斯-牛顿法
while (!converged) {
// 计算雅可比矩阵 J 和残差 e
J = computeJacobian(state);
e = computeResidual(state);
// 求解正规方程
H = J^T * W * J; // W 是信息矩阵
g = -J^T * W * e;
delta = H.solve(g); // 求解线性方程组
// 更新状态
state += delta;
// 检查收敛
if (delta.norm() < threshold) converged = true;
}这里 H 矩阵（海森矩阵的近似）非常稀疏，因为每个观测只关联少数几个状态变量。利用稀疏性，我们可以用舒尔补或C++的稀疏求解器（如CSParse、SuiteSparse）高效求解。
避坑指南：我曾经在优化一个大规模地图时，H矩阵的条件数太大，导致求解发散。后来加了先验约束（比如固定第一个位姿），才稳定下来。记住：非线性优化对初值敏感，初值不好就收敛到局部最优。所以一般先用卡尔曼滤波或里程计给个粗略初值，再用优化精调。
4.5 本章知识体系
下面这张图总结了本章的核心逻辑。从概率论出发，到高斯分布，再到卡尔曼滤波和非线性优化——它们构成了SLAM状态估计的两大流派。
你看，左边是卡尔曼滤波流派，适合在线、实时性要求高的场景；右边是非线性优化流派，适合离线或大规模建图。实际工程中，我经常把两者结合——前端用EKF做快速位姿跟踪，后端用图优化做全局地图修正。
一句话总结：状态估计就是「用概率描述不确定性，用高斯分布简化计算，用卡尔曼滤波做递推估计，用非线性优化做批量优化」。把这四个点串起来，SLAM的数学骨架就有了。
好了，这一章的内容就到这。数学基础打牢了，后面讲具体算法时你会觉得顺很多。记住：公式可以忘，但思想要留下——我们永远在和不确定性打交道。