3. 线性卡尔曼滤波（KF）精讲：状态空间模型、预测与更新步骤的数学推导
好，咱们进入正题。线性卡尔曼滤波，说白了就是一套“预测-修正”的数学框架。你想想看，一个机器人或者一辆自动驾驶的车，它怎么知道自己在哪？它不能全靠传感器，因为传感器有噪声。它也不能全靠模型预测，因为模型有误差。卡尔曼滤波就是干这个的——把两者揉在一起，给出一个最优估计。
我个人习惯把卡尔曼滤波拆成三块来理解：状态空间模型、预测步骤、更新步骤。这三块搞明白了，代码就是顺理成章的事。
3.1 状态空间模型：你得先定义“状态”是什么
做卡尔曼滤波的第一步，不是写公式，而是定义你的系统。我在项目中遇到过不少新手，上来就套公式，结果状态变量都没想清楚，最后滤波结果一塌糊涂。
一个线性系统的状态空间模型长这样：
状态方程：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如何影响状态”。
- u_k：控制输入。比如油门、转向角。
- w_k：过程噪声。服从高斯分布，均值为0，协方差为Q。
- z_k：观测向量。传感器读到的数据。
- H：观测矩阵。它把状态空间映射到观测空间。
- v_k：观测噪声。服从高斯分布，均值为0，协方差为R。
核心要点：A、H、Q、R这四个矩阵，是卡尔曼滤波的“灵魂”。你定义得越准，滤波效果越好。尤其是Q和R，它们代表了“你更相信模型还是更相信传感器”。
举个例子。假设我们有一个一维的匀速运动系统。状态是位置p和速度v：
x_k = [p_k, v_k]^T
状态方程：
p_k = p_{k-1} + v_{k-1} * dt
v_k = v_{k-1}
写成矩阵形式：
x_k = [1, dt; 0, 1] * x_{k-1} + w_k
观测方程（假设我们只观测位置）：
z_k = [1, 0] * x_k + v_k嗯，这里要注意：dt是时间步长。如果你用IMU或者轮式编码器，dt通常就是采样周期。
3.2 预测步骤：先猜一个“大概位置”
预测步骤，说白了就是“根据上一时刻的最优估计，猜一下当前时刻的状态会是什么”。它分两步：
- 状态预测：x_pred = A * x_est_{k-1} + B * u_k
- 协方差预测：P_pred = A * P_{k-1} * A^T + Q
这里x_est_{k-1}是上一时刻的最优估计，P_{k-1}是上一时刻的估计误差协方差。P_pred代表“我们对预测结果有多大的不确定性”。
我刚开始学的时候，总觉得P_pred这个公式很抽象。后来做项目时发现，它其实就是在传播不确定性。你想想看，模型本身有误差（Q），上一时刻的估计也有误差（P_{k-1}），两者叠加，不确定性自然就变大了。
个人经验：在实际工程中，Q矩阵的调参往往是最头疼的。我曾经在一个无人机项目中，因为Q设得太小，导致滤波器过于相信模型，结果无人机一遇到阵风就飘得厉害。后来把Q适当调大，让滤波器更相信GPS观测，问题就解决了。
3.3 更新步骤：用观测数据“修正”预测
预测完了，我们有了一个“先验估计”。但传感器数据来了，我们得用这个观测值去修正预测。更新步骤也分两步：
- 计算卡尔曼增益：K = P_pred * H^T * (H * P_pred * H^T + R)^{-1}
- 状态更新：x_est_k = x_pred + K * (z_k - H * x_pred)
- 协方差更新：P_k = (I - K * H) * P_pred
这里K就是卡尔曼增益。它决定了“预测”和“观测”之间的权重分配。如果K大，说明我更相信观测；如果K小，说明我更相信预测。
为什么会这样？你看公式：
- 如果观测噪声R很小（传感器很准），那么K会变大，滤波器会更多地用观测值去修正预测。
- 如果过程噪声Q很小（模型很准），那么P_pred会变小，K也会变小，滤波器会更依赖预测。
说白了，卡尔曼滤波就是一个“动态加权平均”的过程。权重由Q和R决定。
避坑指南：我曾经在调试一个机器人定位系统时，发现滤波结果发散。查了半天，原来是观测矩阵H写错了。H的维度必须和观测向量z匹配，否则矩阵乘法会出问题。嗯，这种低级错误，犯过一次就记住了。
3.4 完整算法流程：一张图看懂
下面我用一张SVG流程图，把整个KF的预测-更新循环展示出来。你跟着这个流程走一遍，基本就掌握了。
3.5 代码实战：一维匀速运动KF
理论讲完了，咱们直接上代码。下面是一个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]]) # 观测矩阵
Q = np.array([[0.1, 0],
[0, 0.1]]) # 过程噪声协方差
R = np.array([[1]]) # 观测噪声协方差
# 初始状态
x_est = np.array([[0],
[1]]) # 初始位置0，速度1
P = np.eye(2) * 100 # 初始协方差（大一点表示不确定）
# 生成真实轨迹和观测数据
true_states = []
measurements = []
x_true = np.array([[0], [1]])
for k in range(100):
# 真实状态（加入过程噪声）
w = np.random.multivariate_normal([0, 0], Q).reshape(2, 1)
x_true = A @ x_true + w
true_states.append(x_true.flatten())
# 观测（加入观测噪声）
v = np.random.normal(0, np.sqrt(R[0,0]))
z = H @ x_true + v
measurements.append(z.flatten())
# 卡尔曼滤波
estimates = []
for k in range(100):
# 预测
x_pred = A @ x_est
P_pred = A @ P @ A.T + Q
# 更新
z = measurements[k]
K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R)
x_est = x_pred + K @ (np.array([[z[0]]]) - H @ x_pred)
P = (np.eye(2) - K @ H) @ P_pred
estimates.append(x_est.flatten())
# 绘图
estimates = np.array(estimates)
true_states = np.array(true_states)
measurements = np.array(measurements)
plt.figure(figsize=(10, 6))
plt.plot(true_states[:, 0], label='真实位置', color='green')
plt.plot(measurements[:, 0], label='观测位置', color='red', alpha=0.5)
plt.plot(estimates[:, 0], label='估计位置', color='blue')
plt.legend()
plt.xlabel('时间步')
plt.ylabel('位置')
plt.title('一维匀速运动卡尔曼滤波效果')
plt.grid(True)
plt.show()运行建议：你可以试着调整Q和R的值，看看滤波效果怎么变化。比如把R设得很大（传感器噪声大），你会发现估计曲线更平滑，但响应变慢。把Q设得很大（模型不可靠），估计曲线会更贴近观测值，但噪声也更多。
3.6 关键参数调优心得
最后，我总结一下调参的几个要点。这些是我在实际项目中踩过坑之后才明白的：
| 参数 | 物理意义 | 调大效果 | 调小效果 |
|---|---|---|---|
| Q（过程噪声） | 模型的不确定性 | 更相信观测，响应快但噪声大 | 更相信模型，响应慢但平滑 |
| R（观测噪声） | 传感器的不确定性 | 更相信模型，平滑但滞后 | 更相信观测，响应快但抖动 |
| P₀（初始协方差） | 初始状态的不确定性 | 初始收敛快，但初期估计可能震荡 | 初始收敛慢，但更稳定 |
嗯，这里要注意：P₀的初始值不要设得太小。我曾经在一个项目中把P₀设成0，结果滤波器一开始就“锁死”了，后面观测数据来了也不更新。因为P₀=0意味着“我完全相信初始状态”，卡尔曼增益K直接变成0，更新步骤就失效了。
好了，线性卡尔曼滤波的核心内容就这些。你把这个流程和代码吃透了，后面学EKF（扩展卡尔曼滤波）就会轻松很多。因为EKF本质上就是把非线性系统线性化，然后套用KF的框架。