Kalman Filter 交互仿真

卡尔曼滤波(Kalman Filter)由匈牙利裔美国工程师 Rudolf E. Kálmán 于 1960 年提出,最早应用于 Apollo 登月舱的导航系统。它是一种针对线性高斯系统的最优递归状态估计器,能在传感器噪声和模型误差并存的情况下,持续输出状态的最佳估计。

典型应用场景:GPS 定位融合、惯导解算、SLAM 机器人定位、飞行器姿态估计、雷达目标跟踪、金融时间序列预测、神经信号处理……几乎所有涉及"带噪声测量 + 运动模型"的任务,都能看到它的身影。

它的核心魅力在于两点:递归(只需上一时刻的状态,内存占用恒定)和最优(在线性高斯假设下使估计误差的方差最小)。

卡尔曼滤波的每一个时间步都由两步构成:

1. 预测(Predict):根据系统动力学模型,从上一时刻的状态估计外推到当前时刻。同时估计的不确定度会增大——因为模型本身不完美,时间越长越不准。
2. 更新(Update):获得当前时刻的传感器测量后,用它来修正预测值。融合后的不确定度必定减小——因为多了一份信息。

这两步的融合权重,由卡尔曼增益 K 自动计算。K 的大小本质上回答了一个问题:"这次我该更相信模型的预测,还是传感器的测量?"

其中 x̂ 是状态估计,z 是测量值,ẑ 是由预测值推算的测量值。当 K→0,完全信任模型;当 K→1,完全信任测量。

先用最简单的场景建立直觉:估计一个恒定(或缓变)的标量,比如室内温度。状态 x 是温度真值,每一步的预测模型是"温度近似保持不变",传感器给出带噪测量 z = x + 噪声。

用 p 表示当前估计的方差(越大越不确定),Q 是过程噪声方差,R 是测量噪声方差,那么整个滤波器可以写成五行:

看最关键的第 3 式——卡尔曼增益 K = p/(p+R):

• 当 p ≫ R(模型不确定性大,传感器很准),K ≈ 1,几乎完全采纳测量;
• 当 p ≪ R(模型很准,传感器很糙),K ≈ 0,几乎完全信任预测;
• K 的值会随着时间自适应变化,无需手动调节。

实际系统通常有多个状态(位置、速度、姿态角……),需要用状态向量 x 表示。系统的动力学和观测都用矩阵描述:

例:恒速运动模型——一维运动物体,状态为 x = [p, v]ᵀ,采样间隔 Δt,则:

这意味着"下一步的位置 = 当前位置 + 速度 × Δt,速度近似不变"。这也是顶部仿真器默认采用的模型。

每一个时间步,依次执行下面 5 步。左侧 2 步是预测,右侧 3 步是更新:

关键观察:

• 预测步 增加不确定度(+Q),更新步 减少不确定度(乘以 I−KH);
• 增益 K 会自动根据 P 和 R 的相对大小分配权重;
• 整个算法只依赖上一步的 x̂ 和 P,恒定内存、适合嵌入式实时实现;
• 观测更新式中的 z − Hx̂ 称为创新(Innovation),是判断滤波是否发散的重要指标。

Q 和 R 的绝对值不重要,相对比例 Q/R 才决定滤波器的行为。理解它们的物理意义,就掌握了卡尔曼滤波调参的关键:

一句话总结:Q 是"模型的不确定度",R 是"测量的不确定度",卡尔曼滤波做的就是按 P/(P+R) 的比例把两者加权融合。

🧪 两个值得亲自验证的场景:

• 极端对比:Q=5、R=0.01 → 滤波曲线几乎贴着红点乱跳;反之 Q=0.001、R=5 → 曲线极平滑但完全跟不上真值。
• σ ≠ R 的失调:把 σ 设为 0.3(实际噪声很小)但 R 保持 2.0(滤波器却以为噪声很大) → 滤波器会过度平滑,严重滞后——这就是工程中最常见的"R 估计偏大"导致的性能问题。反过来 σ=1.5、R=0.1,滤波器会对噪声过度敏感,输出抖动剧烈。

标准卡尔曼滤波要求系统模型线性 且 噪声高斯。但现实很多任务是非线性的——比如 GPS 将地心坐标转为经纬度、无人机姿态解算、机器人里程计包含 sin/cos、雷达测距测角……一旦出现非线性函数 f(x)、h(x),线性矩阵 F、H 就不存在了。

EKF 的思路:在当前估计点做泰勒一阶线性化。用函数的 Jacobian 矩阵代替 F 和 H:

然后照搬标准卡尔曼滤波的流程即可,只是:

• 状态预测用非线性函数:x̂ = f(x̂_prev),而不再是 Fx;
• 协方差预测用 Jacobian:P = Fₖ P Fₖᵀ + Q;
• 测量残差也用非线性函数:z − h(x̂);
• 其余公式(K、P 更新)不变。

⚠ 注意事项:

• 初值敏感:估计偏离太远时线性化误差大,EKF 可能发散;
• 高非线性失效:对于强非线性系统,建议使用 UKF(无迹卡尔曼滤波)或粒子滤波;
• Jacobian 可能复杂:对于高维状态空间,手推 Jacobian 易出错,可考虑自动微分工具。