第4章：卡尔曼滤波入门：状态空间模型、预测与更新方程、一维卡尔曼滤波手推与代码实现

各位同学，欢迎来到传感器融合的核心章节。今天我们要啃的这块硬骨头，叫卡尔曼滤波。

说实话，我当年刚接触这玩意儿的时候，也是一头雾水。什么状态空间、协方差矩阵，看着就头大。但后来在项目中真正用起来才发现，它其实就是个「猜得准」的数学工具。你想想看，传感器数据有噪声，物理模型有误差，卡尔曼滤波就是那个能把两者揉在一起，给出最优估计的家伙。

4.1 状态空间模型：把问题装进数学框子里

要理解卡尔曼滤波，先得搞懂状态空间模型。说白了，就是把一个动态系统用数学语言描述出来。

一个典型的状态空间模型包含两个核心方程：

  • 状态方程：描述系统状态如何随时间演变
  • 观测方程：描述传感器如何观测到系统状态

拿一个最简单的例子来说——测量温度。假设你有一个恒温箱，温度会缓慢变化，但变化过程有随机扰动。那么状态方程可以写成：

x(k) = x(k-1) + w(k-1)

其中 x(k) 是当前时刻的真实温度，w(k-1) 是过程噪声（比如开门带来的温度波动）。

观测方程呢？你放了个温度传感器在里面：

z(k) = x(k) + v(k)

z(k) 是传感器读数，v(k) 是测量噪声（传感器本身的误差）。

嗯，这里要注意：过程噪声和测量噪声都是随机变量，我们假设它们服从高斯分布。这也是卡尔曼滤波能工作的前提条件之一。

核心思想：状态空间模型把「真实状态」和「观测数据」分开了。我们永远无法直接知道真实状态，只能通过观测去估计它。

4.2 预测与更新方程：卡尔曼滤波的两步走

卡尔曼滤波的精髓，就是两个步骤交替进行：预测（Predict） 和 更新（Update）。

我习惯用一个比喻来理解：你闭着眼睛往前走（预测），然后睁开眼睛看一眼（更新），再闭眼走一步，再睁眼看一眼……每一步都比单靠感觉或单靠眼睛更准。

4.2.1 预测步骤

预测步骤利用状态方程，从上一时刻的最优估计出发，推算出当前时刻的先验估计：

x̂(k|k-1) = x̂(k-1|k-1) + u(k-1)  // 状态预测
P(k|k-1) = P(k-1|k-1) + Q          // 误差协方差预测

这里：

  • x̂(k|k-1) 表示用 k-1 时刻的信息预测 k 时刻的状态
  • P 是误差协方差，衡量估计的不确定性
  • Q 是过程噪声协方差

我在项目中遇到过一个问题：Q 值设得太小，滤波器反应迟钝；设得太大，又容易受噪声干扰。这个参数调起来，确实需要一些经验。

4.2.2 更新步骤

更新步骤利用观测数据，修正预测结果：

K(k) = P(k|k-1) / (P(k|k-1) + R)   // 卡尔曼增益计算
x̂(k|k) = x̂(k|k-1) + K(k) * (z(k) - x̂(k|k-1))  // 状态更新
P(k|k) = (1 - K(k)) * P(k|k-1)     // 误差协方差更新

这里 R 是测量噪声协方差。卡尔曼增益 K 是个关键角色——它决定了你更相信预测还是更相信测量。

个人经验：当 K 接近 1 时，说明测量噪声小，滤波器更信任传感器数据；当 K 接近 0 时，说明过程噪声小，滤波器更信任模型预测。我曾经在一个无人机项目中，因为忘记调整 R 值，导致滤波器完全无视 GPS 数据，差点炸机……

4.3 一维卡尔曼滤波手推：从公式到数字

光看公式不过瘾，我们来手推一个实际例子。假设我们要估计一个恒温箱的温度：

  • 真实温度：25°C（但我们不知道）
  • 初始估计：20°C，误差协方差 P = 5
  • 过程噪声 Q = 0.1
  • 测量噪声 R = 1
  • 第一次测量值：z(1) = 24°C

第一步：预测

x̂(1|0) = x̂(0|0) = 20
P(1|0) = P(0|0) + Q = 5 + 0.1 = 5.1

第二步：更新

K(1) = 5.1 / (5.1 + 1) = 0.836
x̂(1|1) = 20 + 0.836 * (24 - 20) = 23.344
P(1|1) = (1 - 0.836) * 5.1 = 0.836

你看，经过一次更新，估计值从 20°C 跳到了 23.344°C，更接近真实值 25°C 了。误差协方差也从 5.1 降到了 0.836，说明不确定性大大减小。

避坑指南：我曾经在调试时发现，如果初始估计偏差太大，卡尔曼滤波需要好几个周期才能收敛。所以初始值尽量给得靠谱一些，别太离谱。

4.4 代码实现：把理论跑起来

理论说完了，咱们上代码。下面是一个完整的一维卡尔曼滤波实现，用 Python 写的：

import numpy as np
import matplotlib.pyplot as plt

class KalmanFilter1D:
    def __init__(self, x0, P0, Q, R):
        self.x = x0      # 状态估计
        self.P = P0      # 误差协方差
        self.Q = Q       # 过程噪声
        self.R = R       # 测量噪声
    
    def predict(self):
        # 预测步骤
        self.P = self.P + self.Q
        # 状态预测（这里假设系统是恒定的，所以 x 不变）
        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

# 模拟数据
true_temp = 25.0
measurements = true_temp + np.random.randn(50) * 2  # 测量噪声标准差2

# 初始化滤波器
kf = KalmanFilter1D(x0=20.0, P0=5.0, Q=0.1, R=4.0)

# 运行滤波
estimates = []
for z in measurements:
    kf.predict()
    est, _ = kf.update(z)
    estimates.append(est)

# 绘制结果
plt.figure(figsize=(10, 6))
plt.plot(measurements, 'r.', label='测量值', alpha=0.5)
plt.plot(estimates, 'b-', label='滤波估计', linewidth=2)
plt.axhline(y=true_temp, color='g', linestyle='--', label='真实值')
plt.legend()
plt.xlabel('时间步')
plt.ylabel('温度 (°C)')
plt.title('一维卡尔曼滤波效果')
plt.grid(True)
plt.show()

这段代码我用了很多次，每次给学生讲的时候都会强调：Q 和 R 的比值决定了滤波器的行为。你可以试着把 Q 改成 10，看看滤波器会变成什么样子——估计值会剧烈抖动，因为你觉得过程噪声很大，模型不可靠。

核心要点总结：

  • 卡尔曼滤波 = 预测 + 更新，两步循环
  • 卡尔曼增益 K 是核心，平衡模型与测量
  • Q 和 R 需要根据实际系统调参
  • 一维情况是理解多维卡尔曼滤波的基础

说实话，卡尔曼滤波入门不难，难的是在实际系统中调好参数。我建议你先从一维情况开始，把每个公式手推一遍，再用代码跑一遍。等彻底理解了，再去看多维的情况，会轻松很多。

一维卡尔曼滤波核心流程 初始估计 x̂(0), P(0) 预测步骤 (Predict) x̂(k|k-1) = x̂(k-1|k-1) P(k|k-1) = P(k-1|k-1) + Q 更新步骤 (Update) K(k) = P(k|k-1) / (P(k|k-1) + R) x̂(k|k) = x̂(k|k-1) + K(k)·(z(k) - x̂(k|k-1)) P(k|k) = (1 - K(k))·P(k|k-1) 最优估计 x̂(k|k), P(k|k) k → k+1 循环 输入： • 过程噪声 Q • 上一时刻估计 输入： • 测量噪声 R • 传感器读数 z(k)

最后说一句：别怕公式。你把它当成一个「猜数字」的游戏，每次猜完用测量值修正一下，慢慢就上手了。我在实际项目中，从温度估计到姿态解算，从 GPS 定位到视觉跟踪，卡尔曼滤波无处不在。掌握了它，你的传感器融合之路就迈出了最关键的一步。

可可精品资料库

← 上一章 📖 返回目录 下一章 →