4. 状态估计基础：贝叶斯滤波、高斯分布、卡尔曼滤波（KF）推导、扩展卡尔曼滤波（EKF）原理
各位同学，欢迎来到多传感器融合定位的核心章节。说实话，状态估计是整个定位系统的“大脑”。你传感器再多、硬件再贵，如果状态估计做不好，定位结果就是一团浆糊。今天咱们就把这块硬骨头啃下来。
核心思想：状态估计的本质，就是“用带噪声的观测数据，去猜系统真实状态”。说白了，就是在一堆不确定中找确定。
4.1 贝叶斯滤波：一切估计的源头
我个人习惯把贝叶斯滤波看作是状态估计的“宪法”。所有后续的卡尔曼滤波、粒子滤波，都是它的具体实现形式。为什么这么说？因为它给出了一个最根本的框架：用先验知识 + 当前观测 = 后验估计。
公式很简单，但内涵很深：
P(x|z) = [P(z|x) * P(x)] / P(z)这里：
- P(x)：先验概率。在拿到观测之前，你对状态x的猜测。
- P(z|x)：似然概率。给定状态x，观测到z的可能性。
- P(x|z)：后验概率。看到观测z之后，对状态x的修正。
我在项目中遇到过一个问题：只用GPS做定位，信号被高楼遮挡时，定位结果直接飘到马路对面去了。为什么？因为没有利用“车辆不可能瞬间移动”这个先验信息。贝叶斯滤波告诉我们，先验和观测要结合起来，缺一不可。
避坑指南：我曾经在融合IMU和GPS时，忽略了先验的更新。结果就是，IMU的漂移越来越大，而GPS的观测权重始终不变。嗯，这里要注意：贝叶斯滤波是一个递归过程，每一时刻的后验，就是下一时刻的先验。
4.2 高斯分布：为什么它无处不在？
你想想看，为什么几乎所有滤波算法都假设噪声是高斯分布？不是因为它最准确，而是因为它数学上最友好。
高斯分布的几个性质，简直是做状态估计的“神助攻”：
- 线性变换后还是高斯：x ~ N(μ, σ²)，那么 y = Ax + b 也是高斯分布。
- 两个高斯相乘，结果还是高斯：这就是贝叶斯更新能闭式求解的根本原因。
- 边缘分布和条件分布都是高斯：多维高斯分布中，你取一部分变量，或者给定一部分变量求另一部分，结果都是高斯。
我记得刚入行时，导师跟我说：“你只要看到高斯分布，心里就要有数——这问题大概率有解析解。” 后来做EKF、UKF，果然如此。
多维高斯分布的表达式：
P(x) = (1 / sqrt((2π)^n |Σ|)) * exp(-0.5 * (x-μ)^T Σ^(-1) (x-μ))其中μ是均值向量，Σ是协方差矩阵。协方差矩阵里的非对角线元素，刻画了不同状态变量之间的相关性。这个相关性，在融合定位中至关重要。
注意：实际工程中，噪声往往不是严格的高斯分布。比如GPS多路径效应、IMU的温度漂移。这时候强行用高斯假设，结果会偏。我曾经在自动驾驶项目中，因为没处理GPS的非高斯噪声，导致车辆在立交桥下频繁跳变。后来加了鲁棒核函数才解决。
4.3 卡尔曼滤波（KF）推导：从贝叶斯到递推
卡尔曼滤波，说白了就是在线性系统、高斯噪声假设下，贝叶斯滤波的闭式解。它把贝叶斯滤波的积分运算，变成了矩阵乘法和加法。
系统模型：
状态方程：x_k = A * x_{k-1} + B * u_k + w_k, w_k ~ N(0, Q)
观测方程：z_k = H * x_k + v_k, v_k ~ N(0, R)推导过程分两步：预测和更新。
4.3.1 预测步骤
根据上一时刻的后验，预测当前时刻的先验：
x_pred = A * x_est + B * u_k
P_pred = A * P_est * A^T + Q这里P_pred是预测协方差，它比上一时刻的P_est要大——因为加了过程噪声Q。嗯，这很直观：预测的时间越长，不确定性越大。
4.3.2 更新步骤
用当前观测来修正预测：
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x_est = x_pred + K * (z_k - H * x_pred)
P_est = (I - K * H) * P_pred这里K是卡尔曼增益。它的作用就是权衡预测和观测谁更可信。如果观测噪声R很小，K就大，更相信观测；如果过程噪声Q很小（预测很准），K就小，更相信预测。
我的理解：卡尔曼增益K，本质上就是“预测不确定性”和“观测不确定性”的比值。你想想看，如果GPS噪声很大（R大），那K就小，系统更依赖IMU的预测。反之亦然。
我在项目中调试KF时，最常遇到的问题就是Q和R的调参。这两个矩阵设不好，滤波要么发散，要么收敛太慢。我的经验是：先通过离线数据标定R，再根据动态性能要求调整Q。
4.4 扩展卡尔曼滤波（EKF）原理：非线性怎么办？
现实世界哪有那么多线性系统？车辆运动是非线性的，IMU的旋转也是非线性的。这时候，经典KF就不够用了。
EKF的思路很简单：在估计点附近做一阶泰勒展开，把非线性系统近似成线性系统。然后套用KF的框架。
系统模型变成：
状态方程：x_k = f(x_{k-1}, u_k) + w_k
观测方程：z_k = h(x_k) + v_k其中f和h都是非线性函数。EKF的做法是：
- 计算雅可比矩阵：F = ∂f/∂x, H = ∂h/∂x
- 用F和H代替KF中的A和H
- 其余步骤和KF完全一样
避坑指南：我曾经在EKF中忽略了雅可比矩阵的更新频率。系统快速转弯时，线性化点严重偏离真实状态，导致滤波发散。后来我改成在每个时间步都重新计算雅可比，问题就解决了。记住：EKF的线性化误差，是它最大的软肋。
EKF的局限性也很明显：
- 强非线性下，一阶近似误差大
- 雅可比矩阵计算复杂，容易出错
- 系统维度高时，协方差矩阵计算量爆炸
所以后来才有了UKF（无迹卡尔曼滤波）和粒子滤波。但EKF仍然是工业界最常用的方法，因为它简单、稳定、计算量适中。
4.5 本章知识体系图
下面这张图，把本章的核心逻辑串起来了。我建议你多看几遍，理解每个模块之间的依赖关系。
总结一下：贝叶斯滤波是理论框架，高斯分布是数学工具，KF是线性情况下的最优解，EKF是处理非线性的工程妥协。这四者层层递进，构成了多传感器融合定位的理论基石。
好了，这一章的内容就到这里。下一章我们会深入IMU的数学模型和预积分，那才是真正考验工程能力的地方。各位先把今天的内容消化透，后面才能跟得上。
公众号:蓝海资料掘金营，微信deep3321