1. 卡尔曼滤波概述：从贝叶斯滤波到卡尔曼滤波的演进，卡尔曼滤波的核心思想与假设
1.1 为什么我们需要卡尔曼滤波？
做多传感器融合，你一定会遇到一个问题：每个传感器都有噪声。GPS 飘个几米，IMU 零偏慢慢跑，激光雷达偶尔跳点。怎么把这些不完美的数据揉在一起，得到一个靠谱的估计？
卡尔曼滤波就是干这个的。它不完美，但很实用。我在做自动驾驶项目时，第一次把 GPS 和 IMU 用卡尔曼滤波融合，定位精度从米级直接掉到了厘米级。嗯，那感觉确实很爽。
说白了，卡尔曼滤波就是一个递归的估计算法。它根据上一时刻的状态，预测当前时刻的状态，再用当前时刻的观测值去修正这个预测。预测 + 修正，循环往复。
1.2 从贝叶斯滤波说起
要理解卡尔曼滤波，得先知道它的老祖宗——贝叶斯滤波。
贝叶斯滤波的核心思想很简单：
后验概率 ∝ 似然 × 先验概率
用大白话说就是：你本来对某个状态有个猜测（先验），然后你拿到了一组观测数据（似然），你把两者一结合，就得到了更新后的猜测（后验）。
贝叶斯滤波的公式长这样：
预测步：P(x_k | z_{1:k-1}) = ∫ P(x_k | x_{k-1}) P(x_{k-1} | z_{1:k-1}) dx_{k-1}
更新步：P(x_k | z_{1:k}) = P(z_k | x_k) P(x_k | z_{1:k-1}) / P(z_k | z_{1:k-1})这个公式看着吓人，但本质就两步：先猜一下状态会怎么变，再用观测数据去修正。
贝叶斯滤波的问题是——它需要算积分。对于高维状态空间，这个积分基本算不出来。我刚开始学的时候，觉得这玩意儿理论很漂亮，但实际用不了。直到我遇到了卡尔曼滤波。
1.3 卡尔曼滤波的核心思想
卡尔曼滤波对贝叶斯滤波做了三个关键假设，让积分变得可算：
- 线性系统：状态转移和观测都是线性的
- 高斯噪声：过程噪声和观测噪声都是高斯分布
- 马尔可夫性：当前状态只与上一时刻状态有关
有了这三个假设，贝叶斯滤波里的积分就变成了高斯分布的解析运算。你想想看，高斯分布乘高斯分布还是高斯分布，这就好办了。
卡尔曼滤波的五个核心公式：
// 预测步
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这五个公式，我建议你背下来。我在项目中调试卡尔曼滤波时，经常对着这五个公式一行一行地检查代码。
1.4 卡尔曼滤波的假设与局限
卡尔曼滤波虽然好用，但它的假设很严格。我在实际项目中踩过不少坑：
避坑指南：
- 线性假设：系统必须是线性的。如果系统是非线性的，直接用卡尔曼滤波会发散。我曾经在一个无人机项目中，直接用标准卡尔曼滤波处理姿态估计，结果滤波器直接炸了。后来换成扩展卡尔曼滤波（EKF）才搞定。
- 高斯噪声假设：噪声必须是高斯白噪声。如果实际噪声有偏或者有色，卡尔曼滤波的估计会有偏。我记得有一次做室内定位，WiFi信号噪声不是高斯的，卡尔曼滤波的估计结果一直飘，最后用了粒子滤波才解决。
- 模型准确假设：系统模型必须准确。如果模型参数不对，卡尔曼滤波会给出错误的结果。我建议你在实际使用前，先用仿真数据验证一下模型。
1.5 卡尔曼滤波的直观理解
卡尔曼滤波的核心是卡尔曼增益 K。它决定了你更相信预测还是更相信观测：
| 情况 | 卡尔曼增益 K | 含义 |
|---|---|---|
| 观测噪声小 | K 接近 1 | 更相信观测 |
| 观测噪声大 | K 接近 0 | 更相信预测 |
| 预测噪声小 | K 接近 0 | 更相信预测 |
| 预测噪声大 | K 接近 1 | 更相信观测 |
说白了，卡尔曼增益就是一个加权系数。它根据预测和观测的不确定性，自动调整权重。这就是卡尔曼滤波最聪明的地方——它知道什么时候该信谁。
1.6 一个简单的例子
假设你要估计一个物体的位置。你有一个速度传感器（IMU）和一个位置传感器（GPS）。
IMU 的更新频率高（100Hz），但有零偏和漂移。GPS 的更新频率低（10Hz），但绝对精度高。
卡尔曼滤波的做法是：
- 用 IMU 的数据做高频预测（100Hz）
- 有 GPS 数据时，用 GPS 修正预测结果（10Hz）
- 没有 GPS 数据时，只用 IMU 预测
我在做自动驾驶定位时，就是用的这个思路。IMU 负责高频姿态更新，GPS 负责低频位置修正。两者一结合，定位精度和频率都上去了。
个人经验： 卡尔曼滤波的调参是个技术活。Q 矩阵（过程噪声协方差）和 R 矩阵（观测噪声协方差）的取值直接影响滤波效果。我一般先用仿真数据粗调，再用实际数据微调。记住一个原则：Q 越大，滤波器对预测的信任度越低；R 越大，滤波器对观测的信任度越低。
1.7 卡尔曼滤波的变种
标准卡尔曼滤波只适用于线性高斯系统。实际中，我们遇到的大多是非线性系统。这时候就需要它的变种：
- 扩展卡尔曼滤波（EKF）：对非线性函数做一阶泰勒展开，用雅可比矩阵近似线性化。我最早用的就是 EKF，简单粗暴，但要注意雅可比矩阵的计算容易出错。
- 无迹卡尔曼滤波（UKF）：用 sigma 点来近似非线性变换，不需要计算雅可比矩阵。精度比 EKF 高，但计算量也大一些。
- 粒子滤波（PF）：用大量粒子来近似后验分布，适用于非高斯、强非线性系统。计算量最大，但最灵活。
我个人习惯是：如果系统非线性不强，用 EKF；如果非线性强但计算资源够，用 UKF；如果系统极度非线性或者噪声非高斯，用粒子滤波。
1.8 小结
卡尔曼滤波的核心就三件事：
- 用贝叶斯滤波的框架做状态估计
- 用线性高斯假设简化计算
- 用预测-修正的递归结构实现实时处理
它不完美，但很实用。你想想看，一个 1960 年提出的算法，到现在还在广泛使用，这本身就说明了它的价值。
下一章，我会详细讲卡尔曼滤波的数学推导。嗯，别怕，我会用最直观的方式讲清楚。