第四章 卡尔曼滤波基础：状态空间模型、离散卡尔曼滤波（预测与更新）、扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）、卡尔曼滤波在组合导航中的应用框架
各位同学，欢迎来到第四章。这一章，我们要啃一块硬骨头——卡尔曼滤波。
说实话，我刚入行那会儿，看到卡尔曼滤波的公式就头疼。一堆矩阵、一堆协方差，感觉像天书。但后来我发现，这东西说白了就是一个“猜数字”的游戏。你猜一个数，然后拿测量值去修正，再猜下一个。就这么简单。
但在车载定位里，这个“猜数字”的过程，决定了你的定位精度。嗯，咱们今天就把这层窗户纸捅破。
4.1 状态空间模型：你得先知道“状态”是什么
卡尔曼滤波的第一步，是建立状态空间模型。我习惯把它理解成“描述系统怎么动”的数学语言。
一个典型的状态空间模型包含两个方程：
- 状态方程：描述系统状态如何随时间演变。比如，车辆的位置和速度，下一时刻的位置 = 当前位置 + 速度 × 时间间隔 + 噪声。
- 观测方程：描述我们如何通过传感器测量到状态。比如，GPS测到的位置，就是真实位置加上测量噪声。
用数学写出来就是：
x(k) = A * x(k-1) + B * u(k-1) + w(k-1) // 状态方程
z(k) = H * x(k) + v(k) // 观测方程这里，x是状态向量（位置、速度、姿态等），z是观测向量（GPS位置、里程计速度等），A是状态转移矩阵，H是观测矩阵，w和v分别是过程噪声和观测噪声。
关键点：状态空间模型的质量，直接决定了卡尔曼滤波的成败。我在项目中遇到过，有人把状态变量设得太少，结果滤波器发散。也有人设得太多，计算量爆炸。我的建议是：从最核心的变量开始，比如位置、速度、姿态角，然后根据需求逐步增加。
4.2 离散卡尔曼滤波：预测与更新，两步走
离散卡尔曼滤波，是整个卡尔曼家族的基石。它分两步：预测和更新。
预测步：根据上一时刻的状态，猜一下当前时刻的状态。说白了就是“盲猜”。
x_pred = A * x_est + B * u // 状态预测
P_pred = A * P_est * A^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就是“信任度”。如果测量噪声小（R小），K就大，我更相信测量值。如果过程噪声小（Q小），K就小，我更相信预测值。
个人经验：调Q和R矩阵是门手艺活。我曾经在一个项目中，Q设得太小，滤波器反应迟钝，车辆拐弯时定位滞后严重。后来把Q调大了一些，效果立竿见影。记住：Q和R不是随便设的，要根据实际传感器的噪声特性来标定。
4.3 扩展卡尔曼滤波（EKF）：对付非线性问题
现实世界哪有那么多线性系统？车辆的运动是非线性的，GPS的观测也是非线性的。这时候，标准卡尔曼滤波就不够用了。
EKF的思路很简单：把非线性函数线性化。怎么线性化？用泰勒展开，取一阶近似。
比如，状态方程变成了：
x(k) = f(x(k-1), u(k-1)) + w(k-1) // f是非线性函数
// 线性化后：
F = ∂f/∂x (雅可比矩阵)观测方程也一样：
z(k) = h(x(k)) + v(k) // h是非线性函数
// 线性化后：
H = ∂h/∂x (雅可比矩阵)然后，把F和H代入标准卡尔曼滤波的框架里，就完事了。
注意：EKF的线性化是有误差的。如果非线性程度太强，一阶近似可能不够。我曾经在车辆大角度转弯时，EKF的估计误差明显增大。这时候，要么用更高阶的近似，要么换UKF。
4.4 无迹卡尔曼滤波（UKF）：不线性化，直接采样
UKF是另一种思路。它不搞线性化，而是用一组“sigma点”来近似概率分布。
具体做法是：
- 根据当前状态的均值和协方差，生成一组sigma点。
- 把这些sigma点通过非线性函数传播。
- 用传播后的sigma点，重新计算均值和协方差。
这样做的好处是：不需要计算雅可比矩阵，而且对非线性函数的近似精度更高（至少到二阶）。
我个人的习惯是：如果系统非线性不强，用EKF就够了，计算量小。如果非线性强，或者你懒得求导，那就用UKF。UKF的代码写起来反而更规整，因为不用手动算雅可比。
对比总结：
| 方法 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|
| 标准KF | 简单、计算快 | 只适用于线性系统 | 线性模型，如匀速运动 |
| EKF | 适用非线性，计算量适中 | 线性化误差，需计算雅可比 | 弱非线性，如小角度转弯 |
| UKF | 精度高，无需雅可比 | 计算量稍大 | 强非线性，如大角度机动 |
4.5 卡尔曼滤波在组合导航中的应用框架
好了，理论讲完了。咱们来看看在车载定位里，卡尔曼滤波到底怎么用。
组合导航的典型框架是这样的：
- 状态量：位置（经纬度、高度）、速度（东向、北向、天向）、姿态（横滚、俯仰、航向）、以及IMU的零偏误差。
- 输入：IMU的角速度和加速度。
- 观测：GPS的位置和速度（如果有的话）、里程计的速度、或者视觉/激光的位姿。
整个流程是：
- 用IMU数据做状态预测（机械编排）。
- 当GPS或其他传感器数据到来时，做观测更新。
- 修正IMU的误差，输出更准的定位结果。
我建议你记住这个框架：IMU负责高频预测，GPS负责低频修正。两者互补，缺一不可。
避坑指南：我曾经在隧道里测试，GPS信号丢失了十几秒。结果卡尔曼滤波只靠IMU预测，位置误差越飘越大。后来我加了零速检测和里程计约束，才把问题解决。记住：任何传感器都会失效，你的滤波器得有“降级模式”。
好了，这一章的内容就到这里。卡尔曼滤波是组合导航的灵魂，理解了它，你就掌握了车载定位的核心。下一章，咱们会手把手搭建一个完整的组合导航系统。到时候，这些理论都会变成代码。