4. 状态估计基础：贝叶斯估计、最大似然估计、最小二乘估计、卡尔曼滤波原理
各位同学，欢迎来到多传感器融合的核心地带——状态估计。说实话，我做了这么多年融合，最深的体会就是：不懂状态估计，你手里的传感器再多，也只是在收集一堆杂乱无章的数据。今天这一章，咱们就把地基打牢。
我个人习惯把状态估计看作一个「猜谜游戏」。你手里有传感器读数，但真实状态是藏起来的。我们要做的，就是利用数学工具，把那个「最可能」的真实状态给揪出来。下面这四种方法，就是最常用的工具。
4.1 贝叶斯估计：一切估计的「总开关」
贝叶斯估计，说白了就是一套「用新证据更新旧信念」的数学框架。公式很简单：
P(状态 | 观测) = P(观测 | 状态) * P(状态) / P(观测)嗯，这里要注意，分母 P(观测) 通常是个归一化常数，我们更关心分子。我在项目中遇到过不少新手，一上来就纠结分母怎么算，其实没必要。你只要记住：后验概率 ∝ 似然 × 先验。
核心思想：先验知识 + 观测数据 → 后验知识。你之前的经验（先验）越靠谱，融合后的结果就越稳。
举个例子。你有个激光雷达，测距精度 ±5cm。但你事先知道目标不可能在 100m 外突然出现（先验）。贝叶斯框架会自动把这种「常识」融入估计中。这就是为什么纯靠传感器读数容易出问题——它不懂常识。
4.2 最大似然估计：数据说了算
最大似然估计（MLE）是贝叶斯的一个特例。当你对先验一无所知，或者干脆不想用先验时，MLE 就登场了。它的逻辑是：什么样的参数，最有可能产生我看到的这些数据？
公式表达：
θ_MLE = argmax P(观测 | θ)我曾经在一个视觉SLAM项目里，用 MLE 来估计相机位姿。当时手里只有一堆特征点匹配，没有任何先验信息。MLE 直接帮我找到了最「自洽」的位姿。但有个坑——数据量太少时，MLE 容易过拟合。你想想看，如果只有两三个观测点，它找出来的「最可能」参数，很可能只是噪声的产物。
避坑指南：我曾经在室内定位项目中，只用 MLE 估计 WiFi 信号衰减模型，结果因为采样点太少，模型完全跑偏。后来加了贝叶斯先验（比如信号强度不可能突变），才把结果拉回来。
4.3 最小二乘估计：最朴素的「误差最小化」
最小二乘（LS）可能是工程师最熟悉的工具了。它的目标很直白：让预测值和观测值的误差平方和最小。
min Σ (观测_i - 预测_i)^2为什么是平方？不是绝对值？嗯，这里有两个原因：一是平方函数处处可导，方便求极值；二是它天然惩罚大误差——一个 10cm 的误差，平方后是 100，比两个 5cm 的误差（25+25=50）更「严重」。这在传感器融合中很有用，因为大误差往往意味着传感器故障或野值。
我个人习惯在传感器标定阶段先用 LS 打个底。比如 IMU 和相机的联合标定，LS 能快速给出一个不错的初始解。但要注意，LS 假设误差是高斯分布且独立同分布。如果你的传感器有系统性偏差（比如 IMU 零偏），LS 的结果就会偏掉。
警告：最小二乘对野值非常敏感。一个离群的观测点，就能把整个估计结果拉偏。我在做激光雷达和视觉融合时，每次跑 LS 之前，一定会先做野值剔除。否则，结果就是「垃圾进，垃圾出」。
4.4 卡尔曼滤波：动态系统的「最优估计器」
终于到了卡尔曼滤波。说实话，这是多传感器融合里最常用的工具，没有之一。它本质上是一个递归的贝叶斯滤波器，专门处理线性高斯系统。
卡尔曼滤波就两个步骤，循环往复：
- 预测：根据上一时刻的状态，预测当前时刻的状态和不确定性。
- 更新：用当前时刻的观测，修正预测结果。
核心公式（离散形式）：
// 预测步骤
x_pred = A * x_prev + B * u
P_pred = A * P_prev * A^T + Q
// 更新步骤
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x_updated = x_pred + K * (z - H * x_pred)
P_updated = (I - K * H) * P_pred别被公式吓到。你只要理解几个关键量：
- Q（过程噪声协方差）：你对运动模型的信任程度。Q 越大，越相信传感器。
- R（观测噪声协方差）：你对传感器读数的信任程度。R 越大，越不相信传感器。
- K（卡尔曼增益）：自动调节「信模型」还是「信传感器」的权重。
我在一个无人机融合项目中，遇到过 GPS 信号突然变差的情况。卡尔曼滤波的 R 矩阵自动调大（因为 GPS 噪声变大），系统就更多依赖 IMU 的预测。等 GPS 恢复，R 又自动调小。整个过程完全自动，这就是卡尔曼滤波的魅力——它知道什么时候该信谁。
实战要点：调 Q 和 R 是卡尔曼滤波的「艺术」部分。我建议你先用传感器厂商提供的噪声参数作为初始值，然后在实际数据上微调。记住一个原则：Q 和 R 的比值决定了滤波器的响应速度。比值越大，响应越快，但噪声也越大。
4.5 四种方法的对比与选择
讲了这么多，你可能想问：到底该用哪个？我整理了一个对比表，方便你快速决策：
| 方法 | 适用场景 | 优点 | 缺点 |
|---|---|---|---|
| 贝叶斯估计 | 有先验知识，需要完整概率分布 | 理论完备，可融合多源信息 | 计算复杂，高维难处理 |
| 最大似然估计 | 无先验，数据量大 | 简单直接，渐近无偏 | 小样本易过拟合 |
| 最小二乘估计 | 静态标定，离线处理 | 计算快，有闭式解 | 对野值敏感，假设强 |
| 卡尔曼滤波 | 动态系统，实时融合 | 递归高效，自动调权 | 仅限线性高斯系统 |
我个人习惯是：静态问题用 LS，动态问题用 KF，有先验用贝叶斯，没先验用 MLE。当然，实际项目中往往是混合使用。比如先跑 LS 做初始化，再用 KF 做实时跟踪，中间用贝叶斯框架处理异常情况。
一个小技巧：当你对传感器噪声模型不确定时，先用 MLE 从历史数据中估计出噪声参数，再把这些参数喂给卡尔曼滤波。这叫「经验贝叶斯」，实战中非常好用。
好了，状态估计的基础就讲到这里。这四种方法就像工具箱里的四把扳手——没有哪把是万能的，但用对了地方，都能帮你拧紧「融合」这颗螺丝。下一章，我们会深入卡尔曼滤波的变种，看看它是如何应对非线性、非高斯等现实问题的。
公众号:蓝海数据掘金营，微信deep3321