第 4 篇卡尔曼滤波上——用直觉理解 5 个公式卡尔曼滤波是嵌入式领域含金量最高的算法之一。面试常考项目加分无人机、机器人、自动驾驶都在用。但很多人被数学劝退了——这篇只用图讲原理下一篇写代码。1. 一句话理解卡尔曼用预测值和测量值的加权平均但权重卡尔曼增益是动态计算的——谁更可靠就多信谁。假设你在走廊里闭眼走路你的预测步速×时间 我应该走了 10 米 你的测量摸到墙 墙在 12 米处 问题你到底走了多远 如果步速很准预测可信 → 信预测多一点 → 最终估计 ≈ 10.3 米 如果步速不准预测不可信 → 信测量多一点 → 最终估计 ≈ 11.7 米卡尔曼做的事情根据预测的不确定性和测量的不确定性自动算出最优权重。2. 卡尔曼滤波的 5 个公式附直觉解释预测阶段先猜一个公式 1状态预测 x̂⁻[k] A × x̂[k-1] B × u[k] 直觉用上一时刻的状态 控制输入预测当前状态。 例子小车位置 上次位置 速度×dt公式 2协方差预测 P⁻[k] A × P[k-1] × Aᵀ Q 直觉预测的不确定性 上次的不确定性经过系统传递 过程噪声 Q。 Q 的含义系统本身有多乱比如风、摩擦力变化更新阶段用测量修正公式 3卡尔曼增益 K P⁻ × Hᵀ / (H × P⁻ × Hᵀ R) 直觉K 预测的不确定性 / (预测不确定性 测量不确定性) 测量值在最终估计中占多大比重 如果测量噪声 R 很大 → K 变小 → 更信预测 如果预测噪声 P⁻很大 → K 变大 → 更信测量公式 4状态更新 x̂[k] x̂⁻ K × (z - H × x̂⁻) 直觉最终估计 预测值 K ×测量值 - 预测值 这就是加权修正公式 5协方差更新 P[k] (I - K × H) × P⁻ 直觉经过测量修正后不确定性降低了。 如果 K 很大很信测量→ P 大幅减小 如果 K 很小不太信测量→ P 几乎不变3. 一维卡尔曼——用数字跑一遍一维情况下A1, H1, B0公式简单很多预测x̂⁻ x̂ P⁻ P Q 更新K P⁻ / (P⁻ R) x̂ x̂⁻ K × (z - x̂⁻) P (1 - K) × P⁻手算示例测量一个 20℃ 的温度传感器有 ±3℃ 噪声初始: x̂20, P5完全不确定 第 1 次测量 z25℃, Q0.1, R9 (传感器噪声方差3²9) 预测: x̂⁻20, P⁻5.1 K 5.1/(5.19) 0.36 估计: x̂ 20 0.36×(25-20) 21.8℃ ← 偏信了预测K只有0.36 P (1-0.36)×5.1 3.3 第 2 次测量 z24℃ 预测: x̂⁻21.8, P⁻3.4 K 3.4/(3.49) 0.27 估计: x̂ 21.8 0.27×(24-21.8) 22.4℃ P (1-0.27)×3.4 2.5 ← 置信度持续提高 第 3 次测量 z22℃ K 2.6/11.6 0.22 估计: 22.4 0.22×(22-22.4) 22.3℃ 第 4 次测量 z21℃ 估计收敛到 ≈ 21.8℃ ← 接近真实值 22℃关键观察K 从 0.36 → 0.27 → 0.22 逐步减小滤波器越来越自信P 在减小收敛后即使有噪声也不会抖动太大4. Q 和 R 怎么理解最重要的调参Q 过程噪声协方差 → 我有多不信任我的模型 Q 大 → 更信测量值 → 响应快但噪声大 Q 小 → 更信模型预测 → 平滑但响应慢 R 测量噪声协方差 → 我有多不信任传感器 R 大 → 不信任测量值 → 平滑但响应慢 R 小 → 相信测量值 → 响应快但噪声大调参口诀现象原因调整滤波值跟不上真实值Q 太小增大 Q滤波值噪声太大R 太小 / Q 太大增大 R 或减小 Q收敛太慢Q 太小 且 R 太大增大 Q超调震荡Q 太大减小 Q经验值先固定 R用传感器数据手册的噪声方差再调 Q。5. 卡尔曼 vs 一阶低通 vs 滑动平均滑动平均一阶低通卡尔曼代码量15 行5 行30 行内存N×4 字节4 字节20 字节动态权重❌ 固定❌ 固定✅ 自适应需要调参窗口 NαQ, R适用场景慢变信号通用需要最优估计结论大部分场景一阶低通就够了。卡尔曼用在你真的很在意精度的场景——导航、定位、多传感器融合。下一篇卡尔曼滤波下——从一维到多维 C 代码MPU6050 角度估计实战