4. 卡尔曼滤波基础：状态空间模型、卡尔曼滤波原理、预测与更新步骤、简单一维卡尔曼滤波实现
各位同学，欢迎来到传感器融合的核心章节。今天我们要啃的这块骨头，叫卡尔曼滤波。
说实话，我刚开始做机器人抓取控制那会儿，对卡尔曼滤波也是又敬又怕。总觉得这玩意儿数学太深，离工程很远。直到有一次，我在调试一个六轴机械臂的力控抓取——传感器数据抖得像筛糠一样，PID怎么调都稳不住。后来硬着头皮上了卡尔曼滤波，效果立竿见影。嗯，从那以后，我就再也不敢说它「没用」了。
说白了，卡尔曼滤波就是一个「猜」的算法。它帮你从一堆 noisy 的测量值里，猜出系统最真实的状态。你想想看，我们做抓取控制时，力传感器有噪声、视觉有延迟、编码器有量化误差——这些乱七八糟的数据，怎么融合成一个靠谱的力/位估计？卡尔曼滤波就是干这个的。
4.1 状态空间模型：把问题「装进盒子里」
要理解卡尔曼滤波，先得理解状态空间模型。我个人习惯把系统想象成一个「黑盒子」：
- 状态量（State）：盒子里真正的东西，比如当前的位置、速度、力。这是我们想知道的。
- 输入量（Control）：你给盒子施加了什么，比如电机电流、目标位置。
- 观测量（Measurement）：你从盒子上读到的数据，比如编码器读数、力传感器值。
用数学语言写出来，就是两个方程：
状态方程：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)是过程噪声——模型不准确带来的误差z(k)是观测向量H是观测矩阵——状态怎么映射到观测值v(k)是观测噪声——传感器本身的噪声
我在项目中遇到过一个问题：很多同学把状态空间模型想得太复杂。其实你只需要回答三个问题：
- 我的系统状态是什么？（比如：位置、速度）
- 状态怎么随时间变化？（比如：位置 += 速度 * dt）
- 我能测量到什么？（比如：只有位置，没有速度）
回答完这三个问题，A、B、H 矩阵就基本出来了。
4.2 卡尔曼滤波原理：两个「猜」的循环
卡尔曼滤波的核心思想，我总结成一句话：用模型预测，用测量修正。
它有两个步骤，不断循环：
- 预测（Predict）：根据上一时刻的状态和系统模型，猜一下当前状态大概在哪。
- 更新（Update）：拿到传感器测量值后，用这个「实测」去修正刚才的「猜测」。
为什么会这样设计？你想想看，如果你只有模型预测，误差会越积越大（开环嘛）。如果你只相信测量，噪声又会让你跳来跳去。卡尔曼滤波聪明的地方在于——它给预测和测量各算一个「权重」，谁的噪声小，就多信谁一点。这个权重，就是卡尔曼增益 K。
核心直觉：卡尔曼滤波不是「选」预测还是测量，而是「融合」它们。融合的比例由噪声协方差矩阵动态决定。
为了让你更直观地理解，我画了一张流程图：
我的小技巧：刚开始调卡尔曼滤波时，先固定 Q 和 R 的比值。比如 Q/R = 0.1，表示我更相信测量。然后根据滤波效果微调。别一上来就调四个矩阵，会疯的。
4.3 预测与更新步骤：手把手拆解
我们一步步来。假设你有一个一维系统——比如一个滑块在导轨上滑动，你只能测量它的位置（有噪声），但你想同时估计位置和速度。
4.3.1 预测步骤
预测分两步走：
- 状态预测：用上一时刻的状态，按模型推算当前状态。
- 误差协方差预测：同时算一下，这个预测有多「不确定」。
用公式写：
x_pred = A * x_est + B * u
P_pred = A * P_est * A^T + Q这里 P 是误差协方差矩阵，它代表了你对当前估计的「信心」。Q 越大，P_pred 就越大，说明你越不确定这个预测。
注意：Q 不是随便设的。我曾经在一个项目中把 Q 设得太小，结果滤波器几乎不信任测量值，输出曲线平滑得像一条直线——但那是错的！系统实际在震荡，滤波器完全没跟上。Q 太小 = 过度信任模型，会丢失真实动态。
4.3.2 更新步骤
拿到测量值 z 后，开始修正：
- 计算卡尔曼增益 K：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 的公式里，分子是 P_pred（预测的不确定性），分母是 P_pred + R（总不确定性）。如果 R 很小（传感器很准），K 就接近 1，滤波器会更相信测量。如果 R 很大（传感器 noisy），K 就接近 0，滤波器会更相信预测。
说白了，卡尔曼增益就是一个「信任分配器」。
4.4 简单一维卡尔曼滤波实现
光说不练假把式。我们直接写一个一维卡尔曼滤波的 Python 代码。假设我们要估计一个物体的位置，测量值有高斯噪声。
import numpy as np
import matplotlib.pyplot as plt
# 系统参数
dt = 0.1 # 时间步长
# 状态空间模型（一维位置 + 速度）
# 状态向量: [位置, 速度]
A = np.array([[1, dt],
[0, 1]]) # 状态转移矩阵
H = np.array([[1, 0]]) # 观测矩阵（只观测位置）
B = np.zeros((2, 1)) # 无控制输入
# 噪声协方差
Q = np.array([[0.1, 0],
[0, 0.1]]) # 过程噪声
R = np.array([[1]]) # 测量噪声
# 初始状态
x_est = np.array([[0],
[0]]) # 初始位置0，速度0
P_est = np.eye(2) * 100 # 初始不确定性很大
# 模拟真实运动（匀速运动 + 噪声）
true_positions = []
measurements = []
for t in range(100):
true_pos = 0.5 * t * dt # 真实位置
meas = true_pos + np.random.normal(0, np.sqrt(R[0,0])) # 加噪声的测量
true_positions.append(true_pos)
measurements.append(meas)
# 卡尔曼滤波
estimates = []
for z in measurements:
# --- 预测 ---
x_pred = A @ x_est
P_pred = A @ P_est @ A.T + Q
# --- 更新 ---
K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R)
x_est = x_pred + K @ (z - H @ x_pred)
P_est = (np.eye(2) - K @ H) @ P_pred
estimates.append(x_est[0, 0])
# 画图
plt.figure(figsize=(10, 5))
plt.plot(true_positions, label='真实位置', linewidth=2)
plt.plot(measurements, label='测量值', alpha=0.6, marker='o', markersize=3)
plt.plot(estimates, label='卡尔曼估计', linewidth=2, linestyle='--')
plt.legend()
plt.xlabel('时间步')
plt.ylabel('位置')
plt.title('一维卡尔曼滤波效果')
plt.grid(True)
plt.show()这段代码我用了很多次。你跑一下就会发现：测量值（橙色点）跳来跳去，但卡尔曼估计（绿色虚线）非常平滑地跟上了真实轨迹。这就是滤波的魅力。
调试建议：如果你发现估计曲线滞后于真实值，试试增大 Q（让滤波器更相信测量）。如果曲线太抖，试试增大 R（让滤波器更平滑）。记住，Q 和 R 是调参的核心旋钮。
4.5 避坑指南：我踩过的三个坑
最后分享几个实战教训：
- 坑一：忘记初始化 P。P 初始值设得太小，滤波器会「固执己见」，测量值几乎不起作用。我一般设成单位矩阵的 100 倍，让滤波器先「虚心学习」。
- 坑二：Q 和 R 的量纲不匹配。比如位置是米，速度是米/秒，它们的噪声方差差好几个数量级。记得归一化或仔细调参。
- 坑三：认为卡尔曼滤波只能处理线性系统。其实扩展卡尔曼滤波（EKF）和无迹卡尔曼滤波（UKF）可以处理非线性。不过那是下一阶段的内容了。
好了，这一章的内容就到这里。卡尔曼滤波是传感器融合的基石，你把它吃透了，后面讲多传感器融合、力位混合控制时，你会觉得豁然开朗。
可可精品资料库