第2章：状态估计基础
各位同学好，我是老张。今天咱们聊聊状态估计的基础。说实话，这部分内容看着有点数学，但它是整个组合导航和SLAM的根基。我当年刚入行时，就是没把这些基础吃透，结果后面做融合算法时踩了不少坑。
2.1 贝叶斯估计：一切估计的源头
贝叶斯估计，说白了就是利用先验知识和观测数据来推断状态。它的核心公式很简单：
P(x|z) = P(z|x) * P(x) / P(z)这里P(x|z)是后验概率，P(x)是先验概率，P(z|x)是似然。嗯，公式看着简单，但含义很深。
我个人习惯把贝叶斯估计理解成「用新信息更新旧认知」。你想想看，我们做组合导航时，IMU的预测就是先验，GPS的观测就是似然，融合后的结果就是后验。这个框架几乎覆盖了所有状态估计方法。
关键点：贝叶斯估计给出了最优解的理论形式，但实际计算时往往需要近似。因为积分太难算了。
2.2 最大后验估计(MAP)：找最可能的那个
MAP估计的思路很直接：既然后验分布不好求，那我就找让后验概率最大的那个状态值。数学上就是：
x_MAP = argmax P(x|z) = argmax P(z|x) * P(x)注意，分母P(z)跟x无关，可以直接扔掉。这在优化里叫「归一化常数」，我经常跟团队说：别管它，不影响最优解的位置。
我在项目中遇到过一个问题：当先验信息不准时，MAP估计会偏向先验，导致结果偏差。后来我加了一个自适应权重，根据观测质量动态调整先验的信任度，效果好了很多。
避坑指南：我曾经在无人机定位项目里，直接用均匀先验做MAP，结果在GPS信号弱时估计值乱跳。后来改成用IMU短时积分做先验，才稳住。记住：先验不是摆设，它决定了估计的鲁棒性。
2.3 最小二乘估计：最朴素的优化
最小二乘估计，说白了就是让预测值和观测值的误差平方和最小。公式长这样：
x_LS = argmin ||z - h(x)||^2这里h(x)是观测模型。为什么用平方？因为平方函数处处可导，方便求梯度。而且在高斯噪声假设下，最小二乘等价于最大似然估计。
你想想看，最小二乘其实没用到先验信息。它只依赖观测数据。所以当观测数据足够多、噪声是高斯时，它效果很好。但观测少或者有粗差时，它就扛不住了。
我记得有一次做激光雷达标定，用了最小二乘，结果总有几个离群点把结果带偏。后来加了鲁棒核函数（比如Huber核），才把问题解决。
注意：最小二乘对异常值非常敏感。一个粗差就能让结果面目全非。实际工程中一定要做数据预处理，或者用鲁棒最小二乘。
2.4 卡尔曼滤波(KF)：递推的贝叶斯
卡尔曼滤波是贝叶斯估计在线性高斯系统下的精确解。它的核心思想是：预测+更新，两步走。公式如下：
预测步：
x_pred = F * x_prev + B * u
P_pred = F * P_prev * F^T + Q
更新步：
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x_est = x_pred + K * (z - H * x_pred)
P_est = (I - K * H) * P_pred这里K是卡尔曼增益，它决定了你更相信预测还是观测。Q是过程噪声协方差，R是观测噪声协方差。这两个参数调好了，滤波就稳了。
我个人习惯把卡尔曼滤波理解成「带权重的平均」。权重由不确定性决定。不确定性大的，权重就小。这个思想在组合导航里太重要了。
核心洞察：卡尔曼滤波的局限在于线性假设和高斯假设。实际系统往往是非线性的，噪声也不一定是高斯。所以后来才有了EKF、UKF、粒子滤波这些变种。
2.5 四种方法的对比与局限
咱们用一张表来总结一下：
| 方法 | 核心思想 | 优点 | 局限 |
|---|---|---|---|
| 贝叶斯估计 | 后验 = 似然 × 先验 | 理论最优 | 计算复杂，难以精确求解 |
| MAP估计 | 找后验最大的点 | 比贝叶斯好算 | 依赖先验，可能局部最优 |
| 最小二乘 | 误差平方和最小 | 简单、可解析求解 | 对异常值敏感，无先验 |
| 卡尔曼滤波 | 预测+更新递推 | 实时性好，在线性高斯下最优 | 线性假设，高斯假设 |
从这张表能看出，四种方法其实是一脉相承的。贝叶斯是理论框架，MAP和最小二乘是优化方法，卡尔曼滤波是递推实现。在组合导航里，我们经常把卡尔曼滤波和因子图优化结合起来用——卡尔曼做实时估计，因子图做全局优化。
我的建议：初学者不要急着上因子图。先把卡尔曼滤波吃透，理解预测和更新的物理意义。我见过太多人一上来就搞因子图，结果连协方差矩阵怎么更新都搞不清楚。基础不牢，地动山摇。
2.6 本章知识体系图
下面这张图展示了四种方法的关系和演进路径：
从这张图能清楚看到，四种方法层层递进。贝叶斯是理论源头，MAP和最小二乘是具体求解方法，卡尔曼滤波则是递推实现。在组合导航中，我们通常从卡尔曼滤波入手，再过渡到因子图优化。
总结一下：状态估计的核心就是「用不确定性来指导融合」。贝叶斯给出了理论框架，MAP和最小二乘提供了求解手段，卡尔曼滤波实现了在线递推。这些方法各有局限，但组合起来就能应对大部分实际问题。
好了，这一章就到这里。下一章咱们会深入因子图优化的核心——如何用图模型来表示状态估计问题。到时候我会结合一个实际的IMU+GPS融合案例来讲解，敬请期待。