4、卡尔曼滤波入门：状态空间模型、预测与更新步骤，在轨迹平滑中的应用
说到轨迹抖动，很多人的第一反应是「上低通滤波」。但低通滤波有个硬伤——它会引入相位延迟。你想想看，机器人明明已经拐弯了，滤波后的轨迹还在直行，这能忍？
我个人习惯用卡尔曼滤波来处理这类问题。它不光是平滑，还能结合你对系统运动的理解来做「预测」。说白了，它比低通滤波更聪明。
4.1 状态空间模型：你得先描述「系统长什么样」
卡尔曼滤波的核心，是建立一个数学模型来描述系统怎么动、传感器怎么测。这个模型就叫状态空间模型。
它由两个方程组成：
- 状态方程：描述系统状态如何随时间演化
- 观测方程：描述传感器如何测量状态
拿一维轨迹平滑举例。假设我们只关心位置 x 和速度 v。那么状态向量就是：
x_k = [位置, 速度]^T状态方程写成矩阵形式：
x_k = A * x_{k-1} + B * u_k + w_k其中：
A是状态转移矩阵B是控制输入矩阵u_k是控制量（比如加速度指令）w_k是过程噪声，服从高斯分布
观测方程就简单了：
z_k = H * x_k + v_kH 是观测矩阵，v_k 是观测噪声。
关键点：状态空间模型把「系统怎么动」和「传感器怎么测」分开了。你不需要知道传感器内部原理，只需要知道它测的是什么、误差有多大。
4.2 预测与更新：卡尔曼滤波的两步走
卡尔曼滤波就两个步骤，循环往复：
- 预测（Predict）：用模型推算下一时刻的状态
- 更新（Update）：用传感器测量值修正预测结果
我画个流程图，你一看就明白：
嗯，这里要注意：预测步骤用的是模型，更新步骤用的是测量。两者缺一不可。
4.3 核心公式：别怕，就五个
卡尔曼滤波的完整公式就五个。我列个表，方便你对照：
| 步骤 | 公式 | 含义 |
|---|---|---|
| 预测 | x̂ₖ⁻ = A·x̂ₖ₋₁ + B·uₖ |
先验状态估计 |
Pₖ⁻ = A·Pₖ₋₁·Aᵀ + Q |
先验误差协方差 | |
| 更新 | Kₖ = Pₖ⁻·Hᵀ·(H·Pₖ⁻·Hᵀ + R)⁻¹ |
卡尔曼增益 |
x̂ₖ = x̂ₖ⁻ + Kₖ·(zₖ − H·x̂ₖ⁻) |
后验状态估计 | |
Pₖ = (I − Kₖ·H)·Pₖ⁻ |
后验误差协方差 |
我的经验：刚开始学的时候，别纠结矩阵推导。你先把这个五个公式当成「黑盒」用起来，跑通一个例子再说。我当年就是先写代码，后补理论的。
4.4 在轨迹平滑中的应用：从理论到代码
假设我们有一个一维位置传感器，采样频率 100Hz，测量噪声标准差 0.5 米。我们想平滑轨迹。
先定义模型参数：
dt = 0.01 # 采样间隔 10ms
A = [[1, dt],
[0, 1]]
H = [[1, 0]] # 只观测位置
Q = [[0.01, 0],
[0, 0.01]] # 过程噪声协方差
R = [[0.25]] # 观测噪声协方差 (0.5^2)然后循环执行预测和更新：
def kalman_filter(z_meas, x_prev, P_prev):
# 预测
x_pred = A @ x_prev
P_pred = A @ P_prev @ A.T + Q
# 更新
K = P_pred @ H.T @ inv(H @ P_pred @ H.T + R)
x_est = x_pred + K @ (z_meas - H @ x_pred)
P_est = (np.eye(2) - K @ H) @ P_pred
return x_est, P_est跑完整个序列，你就得到了一条平滑的轨迹。而且卡尔曼滤波还能给出速度估计——这在低通滤波里是做不到的。
避坑指南：我曾经在一个项目中把 Q 设得太小，结果滤波结果几乎不信任测量值，轨迹平滑得像直线，但完全跟不上真实运动。反过来，Q 太大又会导致抖动没滤干净。调参是个手艺活。
4.5 卡尔曼滤波 vs 低通滤波：谁更香？
我做个对比，你一看就明白：
| 特性 | 卡尔曼滤波 | 低通滤波 |
|---|---|---|
| 相位延迟 | 小（有预测补偿） | 大（固定延迟） |
| 速度估计 | 天然支持 | 需要额外微分 |
| 模型依赖 | 需要系统模型 | 不需要 |
| 调参难度 | 中等（Q、R 矩阵） | 简单（截止频率） |
| 计算量 | 稍大（矩阵运算） | 极小 |
说白了，如果你的系统模型比较准，卡尔曼滤波是首选。如果只是临时平滑一下，低通滤波也能凑合。
4.6 调参心得：Q 和 R 怎么设？
这是新手最头疼的问题。我分享几个经验：
- R 矩阵：直接从传感器数据手册里查。或者你静态采集一段数据，算方差。
- Q 矩阵：这个比较玄学。我一般先设成
R的 1/10 到 1/100，然后看效果微调。 - 初始 P₀：设大一点没关系，比如
[[100, 0], [0, 100]]。滤波器会自己收敛。
核心原则：Q 控制「你有多相信模型」，R 控制「你有多相信传感器」。Q 越大，滤波结果越贴近测量值；R 越大，结果越平滑但响应越慢。
嗯，卡尔曼滤波入门就到这。你先把状态空间模型搭起来，把五个公式跑通，然后去调 Q 和 R。你会发现，轨迹抖动的问题，其实没那么难解决。