第3章：卡尔曼滤波基础：状态空间模型、预测与更新步骤、一维卡尔曼滤波实现
各位同学，欢迎来到传感器融合的核心章节。说实话，卡尔曼滤波这玩意儿，我刚入行时也觉得它像个黑盒子——输入输出挺玄乎。但后来在项目里被噪声折磨过几次，才真正体会到它的妙处。今天我们就把它拆开，看看里面到底装了啥。
3.1 状态空间模型：把问题装进数学框子里
先问大家一个问题：你如何描述一辆车在某一时刻的状态？
我个人习惯用「状态向量」来搞定。比如一辆车在直道上跑，我关心的无非是两个东西：位置和速度。把它们写成一个列向量，就是状态空间模型的基础。
状态向量定义：
x = [位置, 速度]^T其中，位置单位是米，速度单位是米/秒。
有了状态，还得描述它怎么变。这就是状态转移方程。说白了，就是根据上一时刻的状态，猜一猜当前时刻的状态。
举个例子：假设采样时间间隔是 Δt，那么：
位置_new = 位置_old + 速度_old * Δt
速度_new = 速度_old （假设匀速运动）写成矩阵形式就是：
x_k = F * x_{k-1} + B * u_k + w_k这里：
- F 是状态转移矩阵，描述系统自身的演化规律
- B 是控制输入矩阵，比如你踩油门给的控制量
- u_k 是控制向量
- w_k 是过程噪声——嗯，现实世界总是不完美的
我在项目中遇到过一种情况：有人把 F 矩阵写错了，结果滤波器发散得一塌糊涂。你想想看，模型都不对，滤波怎么可能准？
3.2 预测与更新：卡尔曼滤波的两板斧
卡尔曼滤波的核心就两个步骤：预测和更新。我习惯叫它「猜」和「修」。
3.2.1 预测步骤（猜）
这一步，我们用系统模型去估计当前的状态。说白了就是：根据上一时刻的最优估计，猜一猜现在应该在哪。
// 预测状态
x_pred = F * x_est + B * u
// 预测误差协方差
P_pred = F * P_est * F^T + Q这里：
- x_pred 是预测的状态
- P_pred 是预测的误差协方差——它代表了你对这次猜测有多大的信心
- Q 是过程噪声协方差矩阵，描述模型本身的不确定性
我的小技巧：Q 矩阵的调参是个手艺活。调太大，滤波器会过于相信测量值，噪声抑制效果差；调太小，滤波器反应迟钝。我一般先从物理意义出发估算一个初值，再在实际数据上微调。
3.2.2 更新步骤（修）
有了预测，我们还得用传感器测量值来修正它。这一步就是「修」。
// 计算卡尔曼增益
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 是卡尔曼增益——它是预测和测量之间的权重分配器
- H 是观测矩阵，把状态空间映射到测量空间
- z 是实际测量值
- R 是测量噪声协方差矩阵
为什么会这样？说白了，卡尔曼增益 K 决定了你更相信模型预测还是传感器测量。如果测量噪声小（R 小），K 就大，滤波器会更相信测量值；反之亦然。
3.3 一维卡尔曼滤波实现：从理论到代码
理论讲完了，我们来点实际的。一维卡尔曼滤波是最简单的形式，但麻雀虽小五脏俱全。我在调试激光雷达测距数据时，就经常用它来做预处理。
假设我们要估计一个物体的位置（一维），测量值来自一个噪声较大的传感器。
一维卡尔曼滤波完整实现：
# 一维卡尔曼滤波类
class KalmanFilter1D:
def __init__(self, x0, P0, Q, R):
self.x = x0 # 初始状态估计
self.P = P0 # 初始误差协方差
self.Q = Q # 过程噪声方差
self.R = R # 测量噪声方差
def predict(self, dt=1.0):
# 预测步骤
# 假设匀速运动：x = x + 0 * dt（这里简化了）
# 实际上对于一维位置估计，我们常用 x = x
self.P = self.P + self.Q * dt
return self.x
def update(self, z):
# 更新步骤
K = self.P / (self.P + self.R)
self.x = self.x + K * (z - self.x)
self.P = (1 - K) * self.P
return self.x, K
# 使用示例
kf = KalmanFilter1D(x0=0.0, P0=1.0, Q=0.01, R=0.1)
# 模拟测量数据
measurements = [1.2, 1.8, 2.1, 2.7, 3.0, 3.5, 4.2]
for z in measurements:
kf.predict(dt=0.1)
x_est, K = kf.update(z)
print(f"测量值: {z:.2f}, 估计值: {x_est:.2f}, 卡尔曼增益: {K:.3f}")我曾经踩过的坑：一开始我把 Q 设成了 0，觉得模型很完美。结果滤波器完全不响应测量值的变化，估计值一直滞后。后来才意识到，Q 不能为 0，它代表了模型的不确定性——哪怕你的模型再精确，现实世界总有意外。
3.4 卡尔曼滤波核心逻辑流程图
下面这张图是我自己画的核心逻辑，把预测和更新的循环关系展示清楚了。你看一遍应该就能记住整个流程。
3.5 参数调优：让滤波器真正好用
代码写完了，但要让滤波器真正好用，参数调优是关键。我总结了一张表，方便大家对照：
| 参数 | 物理含义 | 调大效果 | 调小效果 |
|---|---|---|---|
| Q | 过程噪声方差 | 更相信测量，响应快但噪声大 | 更相信模型，平滑但滞后 |
| R | 测量噪声方差 | 更相信模型，平滑但滞后 | 更相信测量，响应快但噪声大 |
| P₀ | 初始误差协方差 | 初始收敛慢 | 初始收敛快，但可能不稳定 |
我的调参经验：先根据传感器手册给的噪声指标设定 R，再根据你对模型的信心设定 Q。如果不知道从哪开始，试试 Q = 0.01, R = 0.1 这个组合，很多场景下都能用。
嗯，到这里卡尔曼滤波的基础就讲完了。记住三个核心：状态空间模型是骨架，预测和更新是两板斧，一维实现是练手的好起点。下一章我们会把它扩展到多维，到时候你就知道今天打的基础有多重要了。