4. 多传感器融合基础：卡尔曼滤波入门
各位同学，今天我们来聊聊多传感器融合里最经典、也最实用的一个工具——卡尔曼滤波。
说实话，我刚入行那会儿，一听到「滤波」两个字，脑子里全是低通、高通、带阻这些信号处理的概念。后来在机器人项目里真正上手做定位融合，才发现卡尔曼滤波完全不是那回事。它更像是一个「状态估计器」，帮你从一堆带噪声的测量数据里，把真实状态给「猜」出来。
4.1 为什么需要卡尔曼滤波？
你想想看，在SLAM或者机器人导航里，我们手头有什么？
- IMU：加速度计、陀螺仪，短时间准，但长时间飘得厉害
- 轮式里程计：打滑就完蛋
- GPS：室外还行，室内直接罢工
- 激光雷达/视觉：特征少的时候，定位直接崩
每个传感器都有自己的脾气。单独用哪一个，都不靠谱。那怎么办？
卡尔曼滤波的思路很简单：把多个传感器的信息「加权平均」一下。但这个「加权」不是随便加的，它根据每个传感器当前的不确定性，动态调整权重。
核心思想：预测 + 测量 = 最优估计
说白了，就是「我猜你现在在哪」加上「我测到你在哪」，然后取一个最靠谱的中间值。
4.2 卡尔曼滤波的两大步骤
卡尔曼滤波其实就两个阶段，来回循环：
4.2.1 预测阶段（Predict）
这一步，我们用系统的运动模型，从上一时刻的状态，推算出当前时刻的状态。
比如，你上一秒在位置 x₁，速度 v，时间间隔 Δt，那这一秒的位置大概就是：
x₂ = x₁ + v * Δt当然，实际系统会有控制输入（比如你踩了油门），也会有过程噪声（比如风阻、路面不平）。
公式长这样：
x̂ₖ⁻ = A * x̂ₖ₋₁ + B * uₖ₋₁
Pₖ⁻ = A * Pₖ₋₁ * Aᵀ + Q这里：
x̂ₖ⁻是预测的状态A是状态转移矩阵B是控制输入矩阵u是控制量P是协方差矩阵（代表不确定性）Q是过程噪声协方差
我的经验：Q矩阵的调参，是卡尔曼滤波里最玄学的地方之一。调大了，滤波太「信测量」；调小了，又太「信模型」。我一般先在仿真里跑一遍，看看残差分布，再手动微调。
4.2.2 更新阶段（Update）
预测完了，我们得拿实际测量数据来修正一下。
比如，激光雷达告诉你，你现在在位置 z。但你的预测位置是 x̂ₖ⁻。这两个有偏差，怎么办？
卡尔曼滤波引入了一个「卡尔曼增益」K，来决定你更信谁：
Kₖ = Pₖ⁻ * Hᵀ * (H * Pₖ⁻ * Hᵀ + R)⁻¹
x̂ₖ = x̂ₖ⁻ + Kₖ * (zₖ - H * x̂ₖ⁻)
Pₖ = (I - Kₖ * H) * Pₖ⁻这里：
H是观测矩阵（把状态空间映射到测量空间）R是测量噪声协方差zₖ是实际测量值
你看，当测量噪声 R 很小（传感器很准）时，K 会变大，滤波更相信测量。反之，当预测的 P 很小时，K 变小，更相信模型预测。
注意：我曾经在一个项目里，把R设得太小，结果GPS一丢，滤波直接跟着IMU的漂移飞出去了。后来加了异常检测，测量值偏差太大时，直接跳过更新步骤。
4.3 一个完整的卡尔曼滤波流程
为了让你更直观地理解，我画了一张流程图：
嗯，这张图把整个流程串起来了。每次迭代，先预测，再更新，然后输出当前的最优估计，接着进入下一时刻。
4.4 一个简单的代码示例
光讲理论不过瘾，我们直接上代码。这是一个一维位置估计的卡尔曼滤波实现：
import numpy as np
class KalmanFilter1D:
def __init__(self, x0, P0, Q, R):
self.x = x0 # 初始状态
self.P = P0 # 初始协方差
self.Q = Q # 过程噪声
self.R = R # 测量噪声
def predict(self, u=0.0, dt=1.0):
# 简单运动模型：x = x + v*dt，这里假设速度恒定
A = np.array([[1.0, dt],
[0.0, 1.0]])
B = np.array([[0.0],
[1.0]])
self.x = A @ self.x + B.flatten() * u
self.P = A @ self.P @ A.T + self.Q
def update(self, z):
H = np.array([[1.0, 0.0]])
y = z - H @ self.x
S = H @ self.P @ H.T + self.R
K = self.P @ H.T / S
self.x = self.x + K.flatten() * y
self.P = (np.eye(2) - K @ H) @ self.P
# 使用示例
kf = KalmanFilter1D(x0=np.array([0.0, 0.0]),
P0=np.eye(2) * 100,
Q=np.eye(2) * 0.1,
R=np.array([[1.0]]))
measurements = [1.0, 2.1, 3.0, 4.2, 5.1]
for z in measurements:
kf.predict(u=0.1)
kf.update(z)
print(f"位置: {kf.x[0]:.3f}, 速度: {kf.x[1]:.3f}")避坑指南：我曾经在初始化P0时设得太小，结果滤波收敛特别慢，前几十帧基本不更新。后来我习惯把P0设大一些（比如100），让滤波先「大胆探索」，再慢慢收敛。
4.5 卡尔曼滤波的适用条件
卡尔曼滤波不是万能的。它有几个前提：
| 条件 | 说明 |
|---|---|
| 线性系统 | 状态转移和观测都必须是线性的 |
| 高斯噪声 | 过程噪声和测量噪声都服从高斯分布 |
| 噪声统计已知 | Q和R矩阵需要提前知道或估计 |
如果系统是非线性的（比如机器人转弯、IMU姿态解算），那就得用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）。这个我们后面章节再聊。
注意：别以为卡尔曼滤波能解决所有融合问题。我见过有人硬把非线性系统套进标准KF，结果滤波发散得一塌糊涂。该用EKF的时候，别偷懒。
4.6 小结
卡尔曼滤波的核心，就是「预测 + 测量 = 最优估计」。它用协方差矩阵来量化不确定性，用卡尔曼增益来动态调整权重。
我个人觉得，理解卡尔曼滤波最好的方式，就是动手写一遍代码，然后调参看看效果。你会发现，Q和R的比值，直接决定了滤波的「性格」——是更平滑还是更灵敏。
好了，这一章就到这里。下一章我们聊聊扩展卡尔曼滤波，看看怎么处理那些不听话的非线性系统。