4. 卡尔曼滤波入门：状态空间模型、五大核心公式推导、一维Python实现
各位同学好，我是你们这期课程的讲师。今天咱们来啃一块硬骨头——卡尔曼滤波。
说实话，我当年刚接触这玩意儿的时候，也是一头雾水。什么状态空间、协方差矩阵，看得人脑壳疼。但后来在项目里被逼着用了几次，才发现它其实没那么玄乎。说白了，卡尔曼滤波就是一个帮你从一堆不靠谱的测量数据里，把真实状态“猜”出来的数学工具。
你想想看，我们做传感器融合，每个传感器都有噪声。GPS飘个几米，IMU有零偏，这都是家常便饭。卡尔曼滤波就是那个能把所有信息揉在一起，给出一个最优估计的“和事佬”。
4.1 状态空间模型：先搞清楚我们在算什么东西
在讲公式之前，咱们得先统一一下语言。卡尔曼滤波里最核心的概念，就是状态空间模型。
什么叫状态？就是你想知道的那个东西。比如小车的速度、位置，或者电池的SOC。状态空间模型，就是用数学方程来描述这个状态是怎么随时间变化的，以及我们是怎么观测到它的。
它由两个方程组成：
- 状态方程：描述状态如何从上一时刻演变到当前时刻。
- 观测方程：描述当前时刻的状态如何产生观测值。
写成数学形式就是：
x(k) = A * x(k-1) + B * u(k) + w(k)
z(k) = H * x(k) + v(k)这里：
x(k)：k时刻的状态向量A：状态转移矩阵，描述状态怎么变B：控制输入矩阵，描述外部控制的影响u(k)：控制输入w(k)：过程噪声，服从高斯分布 N(0, Q)z(k)：观测值H：观测矩阵，描述状态到观测的映射v(k)：观测噪声，服从高斯分布 N(0, R)
核心理解：状态方程是我们的“模型预测”，观测方程是我们的“传感器读数”。卡尔曼滤波就是在这两者之间找平衡。
我在做无人机姿态估计时，状态量就是四元数和角速度偏差。A矩阵描述的是刚体运动学，H矩阵描述的是加速度计和磁力计的观测模型。嗯，这里面的门道很多，但基础框架就是这个。
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 就大，我们更相信传感器测量。
说白了，K 就是一个权重系数。谁靠谱，就多信谁一点。
我的经验：调卡尔曼滤波，80%的时间都在调 Q 和 R 这两个参数。Q 设大了，滤波结果会跟着测量值剧烈跳动；R 设大了，结果会变得迟钝。找到一个平衡点，需要反复试。
我曾经在一个车载定位项目里，GPS信号偶尔会丢。一开始我把 R 设得很小，结果GPS一恢复，位置就猛跳一下。后来我把 R 设成动态的——信号好的时候小，信号差的时候大，效果就好多了。
4.3 一维卡尔曼滤波Python实现：动手试试
理论讲完了，咱们来点实际的。下面是一个一维卡尔曼滤波的Python实现，用来估计一个匀速运动物体的位置。
import numpy as np
import matplotlib.pyplot as plt
class KalmanFilter1D:
def __init__(self, Q=0.01, R=0.1):
# 初始状态
self.x = 0.0 # 位置
self.P = 1.0 # 初始协方差
# 系统参数
self.A = 1.0 # 状态转移矩阵（一维匀速）
self.H = 1.0 # 观测矩阵
self.Q = Q # 过程噪声协方差
self.R = R # 观测噪声协方差
def predict(self):
# 状态预测
self.x = self.A * self.x
# 协方差预测
self.P = self.A * self.P * self.A + self.Q
return self.x, self.P
def update(self, z):
# 卡尔曼增益
K = self.P * self.H / (self.H * self.P * self.H + self.R)
# 状态更新
self.x = self.x + K * (z - self.H * self.x)
# 协方差更新
self.P = (1 - K * self.H) * self.P
return self.x, self.P
# 模拟数据
true_positions = np.arange(0, 100, 0.1) # 真实位置
measurements = true_positions + np.random.normal(0, 1, len(true_positions)) # 带噪声的测量
# 运行卡尔曼滤波
kf = KalmanFilter1D(Q=0.01, R=1.0)
estimates = []
for z in measurements:
kf.predict()
est, _ = kf.update(z)
estimates.append(est)
# 绘制结果
plt.figure(figsize=(12, 6))
plt.plot(true_positions, label='真实位置', linewidth=2)
plt.plot(measurements, label='测量值', alpha=0.5)
plt.plot(estimates, label='卡尔曼估计', linewidth=2)
plt.legend()
plt.xlabel('时间步')
plt.ylabel('位置')
plt.title('一维卡尔曼滤波效果')
plt.grid(True)
plt.show()注意：这个例子是最简单的一维情况。实际项目中，状态向量可能是多维的，矩阵运算会复杂很多。但核心思想完全一样。
运行这段代码，你会看到卡尔曼估计的曲线比测量值平滑得多，而且更接近真实值。这就是滤波的效果。
4.4 知识体系总览
为了让你对本章内容有个整体把握，我画了一张图：
这张图把本章的内容串起来了。从左到右，从理论到实践，一步步深入。
避坑指南：我曾经在调一个多传感器融合项目时，Q和R设得不对，结果滤波结果发散，直接飞了。后来花了整整两天才找到问题——原来是Q矩阵里的一个元素设大了两个数量级。所以，调参的时候一定要细心，最好先把每个参数的影响单独测试一遍。
好了，这一章的内容就到这儿。卡尔曼滤波是个大话题，咱们今天只开了个头。后面的章节会逐步深入，包括多维卡尔曼滤波、扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）等等。
记住，理解卡尔曼滤波的关键，就是理解“预测”和“更新”这两个步骤，以及卡尔曼增益K的物理意义。把这个搞明白了，后面的东西就水到渠成了。