4、卡尔曼滤波基础：状态空间模型、预测与更新方程、协方差矩阵的物理意义
各位做多传感器融合的同行，今天我们来啃一块硬骨头——卡尔曼滤波。说实话，我刚入行那会儿，看到一堆矩阵公式就头大。但做了几年定位算法后，我越来越觉得这东西太重要了。你想想看，我们手里有GPS、IMU、轮速计，每个传感器都有噪声，怎么把它们揉在一起得到更准的位置？卡尔曼滤波就是干这个的。
说白了，卡尔曼滤波就是一个“预测-修正”的循环。它不要求你一次算准，而是边走边调整。嗯，这跟咱们做工程解决问题的思路很像——先有个大概估计，然后拿观测数据去修正。
4.1 状态空间模型：你的系统长什么样？
做卡尔曼滤波的第一步，就是定义你的系统状态。我个人习惯用状态空间模型来描述系统，它长这样：
x_k = A * x_{k-1} + B * u_k + w_k
z_k = H * x_k + v_k别被符号吓到，咱们拆开来看：
- x_k：k时刻的系统状态。比如位置、速度、姿态角。这就是你想估计的东西。
- A：状态转移矩阵。描述上一时刻的状态怎么变到这一时刻。比如匀速运动模型，位置 = 位置 + 速度 * dt。
- B：控制输入矩阵。u_k是外部控制量，比如你给IMU的加速度指令。
- w_k：过程噪声。模型不完美带来的误差，比如你假设匀速，但实际有加速。
- z_k：观测值。传感器直接读到的数据，比如GPS的位置。
- H：观测矩阵。把状态映射到观测空间。比如你的状态是位置和速度，但GPS只测位置，H就是[1, 0]。
- v_k：观测噪声。传感器本身的测量误差。
核心理解：状态空间模型就是把“系统怎么动”和“传感器怎么测”这两件事分开建模。你不需要知道传感器内部怎么工作的，只需要知道它测的是什么、误差有多大。
我在项目中遇到过一个问题：有人把IMU的加速度直接当状态用，结果模型发散。为什么？因为加速度变化太快，用线性模型根本预测不准。后来我改成用位置和速度做状态，加速度当控制输入，问题就解决了。
4.2 预测方程：先猜一个
卡尔曼滤波的第一步是预测。根据上一时刻的最优估计，推算当前时刻的状态。公式如下：
x_pred = A * x_est_{k-1} + B * u_k
P_pred = A * P_{k-1} * A^T + Q这里：
- x_pred：预测的状态（先验估计）
- P_pred：预测的协方差矩阵（先验协方差）
- Q：过程噪声协方差矩阵。代表你对模型的信任程度。
你可能会问：为什么还要算P？嗯，这很重要。P代表你对当前估计的不确定度。Q越大，P增长越快，说明你越不信任模型，后面会更依赖观测数据。
个人经验：调Q矩阵时，我建议先从对角线元素开始。比如位置的过程噪声设小一点（0.01），速度的设大一点（0.1）。因为位置变化相对平滑，速度受加速度影响波动大。我曾经把Q设得太大，结果滤波结果跟着GPS噪声乱跳，后来调小了才稳定。
4.3 更新方程：用观测修正
预测完了，现在手里有观测数据z_k。我们要用这个数据来修正预测值。更新方程如下：
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：观测噪声协方差矩阵。代表你对传感器的信任程度。
- z_k - H * x_pred：新息（innovation）。观测值和预测值的差异。
卡尔曼增益K决定了你更相信预测还是观测。K大，说明观测噪声小，你更信传感器；K小，说明过程噪声小，你更信模型。说白了，这就是一个加权平均的过程。
避坑指南：我曾经在融合GPS和IMU时，把R设得太小（太相信GPS），结果GPS一丢信号，位置直接飞了。后来我根据GPS的定位精度（比如标准差）动态调整R，信号好时R小，信号差时R大，效果好了很多。
4.4 协方差矩阵的物理意义
协方差矩阵P是卡尔曼滤波里最容易被忽视的东西。很多人只关注状态估计值，不看P。但我觉得，P才是滤波器的“健康指标”。
咱们拿二维位置估计举个例子。假设状态是[x, y]^T，那么P矩阵是2x2的：
P = | σ_x^2 σ_xy |
| σ_xy σ_y^2 |- 对角线元素：σ_x^2和σ_y^2。代表x和y方向的不确定度。值越大，说明你越不确定。
- 非对角线元素：σ_xy。代表x和y之间的相关性。如果σ_xy > 0，说明x偏大时y也容易偏大。
我习惯把P矩阵画成误差椭圆。椭圆的长轴和短轴对应P的特征向量和特征值。椭圆越大，说明不确定度越大；椭圆越扁，说明某个方向上的不确定度更小。
实战技巧：在融合定位中，我经常监控P矩阵的迹（对角线元素之和）。如果迹突然变大，说明滤波器可能发散，需要重置或调整参数。有一次在隧道里，GPS信号全丢，P的迹涨了10倍，我立刻切换到纯IMU推算模式，等出隧道再融合回来。
4.5 知识体系总览
下面这张图总结了卡尔曼滤波的核心逻辑。我建议你把它记在脑子里：
这张图展示了卡尔曼滤波的完整流程。从上一时刻的最优估计出发，经过预测步骤得到先验估计，然后结合观测数据进行更新，得到当前时刻的最优估计。这个循环不断重复，每一步都在降低不确定性。
4.6 一个简单的例子
假设我们要估计一维位置。状态只有位置x，速度v。状态向量是[x, v]^T。假设匀速运动模型：
A = [[1, dt],
[0, 1]]
H = [[1, 0]] # 只观测位置
Q = [[0.01, 0],
[0, 0.1]] # 位置噪声小，速度噪声大
R = [[0.5]] # 观测噪声标准差0.7左右初始化：x_0 = [0, 0]^T, P_0 = [[1, 0], [0, 1]]
然后循环执行预测和更新。你会发现，随着观测数据增多，P矩阵的对角线元素会逐渐减小并收敛。这就是卡尔曼滤波在“学习”系统的过程。
调试建议：如果你发现滤波结果震荡，先检查P矩阵是否收敛。如果P一直不收敛，说明你的Q和R设置不合理。我一般先固定R（根据传感器手册），然后调Q，直到P收敛到稳定值。
好了，卡尔曼滤波的基础就讲到这里。说白了，它就是一套“预测-修正”的数学框架。状态空间模型帮你描述系统，预测方程给你一个先验估计，更新方程用观测数据修正，协方差矩阵告诉你估计的可信度。掌握了这些，你就能理解多传感器融合的核心思想了。