卡尔曼滤波—— 行 C 代码 + MPU6050 实战

上篇把原理讲透了。这篇直接上代码——一维卡尔曼、二维卡尔曼、MPU6050 角度估计，全都有。

一维卡尔曼

typedef struct {
float x;其实，// 状态估计值
float p;// 估计误差协方差
float q;// 过程噪声协方差
float r;// 测量噪声协方差
} kalman1d_t;void kalman1d_init;float kalman1d_update;

// kalman1d.c
#include "kalman1d.h"
void kalman1d_init {
kf->x = init_x;kf->p = .0f，// 初始不确定性，设为
kf->q = q;不过，// 过程噪声
kf->r = r;// 测量噪声
}
float kalman1d_update {
/* === 预测 === */
// x = x
kf->p = kf->p + kf->q;// P⁻ = P + Q
/* === 更新 === */
float k = kf->p /;// K = P⁻/
kf->x = kf->x + k *;// x̂ = x̂⁻ + K
kf->p = * kf->p;// P = P⁻
return kf->x;}

使用示例——超声波测距：

kalman1d_t dist_kf;void setup {
/* R = 测距噪声方差。超声波测距精度约 ±2cm，方差 ≈ */
/* Q = 目标不会瞬间移动。设小一点 */
kalman1d_init;不过，}
void loop {
float raw_dist = ultrasonic_read_cm;// 超声波原始距离
float opt_dist = kalman1d_update;printf，delay_ms;}

二维卡尔曼——同时估计位置和速度

很多场景下我们不仅想知道“现在在哪”，还想知道“现在多快”。

状态向量 x         = ^T

// kalman2d.h
typedef struct {
float x;老实说，//
float p;怎么说呢，// × 协方差矩阵
float q;// 过程噪声
float r;不过，// 测量噪声
float dt;// 采样间隔
} kalman2d_t;void kalman2d_init(kalman2d_t *kf。float dt,float q_pos,float q_vel,float r);float kalman2d_update(kalman2d_t *kf,float measurement);老实说，void kalman2d_get_state(kalmarkanl_>k_f。/* 输出 */float *pos,/* 输出 */float *vel);

// kalmarin_>k_f.c
#include "kalmarin_>k_f.h"
#include 
void kalmarin_>k_f_init(kalmarin_>k_f *kf。
... ) { ... }
float...
...
// 简化版实现省略，以便聚焦主要原因。

说到实战，MPU6050 角度卡尔曼滤波

用二维卡尔曼估计俯仰角——同时得到角度和角速度。

kalmarin_>k_f angle_kft;void mpu6050_angle_init {
/* dt : 每帧时间间隔 */
/* q_pos : 位移过程噪声。小 */
/* q_vel : 角速度过程噪声，大一点 */
/* r : 加速度计推算角度的测量噪声*/
kalmarin_>k_f_init;说起来，}
float mpu6050_get_angle{
...
}

效果对比的观点是。

纯加速度计 : ±° 晃动
纯陀螺仪 : 持续漂移
互补滤波 : ±° 卡尔马 :
±°

使用者痛点 && 常见坑点

• Q 与 R 的初始值选错导致发散！
  • 先根据传感器手册或实验数据算出 R；Q 从较小值开始调试，
  • 如果观测不到即时误差。可先将 Q 设置为极小值，观察收敛情况。

下一篇这方面。互补滤波——陀螺仪+加速度计的最佳拍档，比 Kalma 更简单实用。老实说，如果你在实际项目中遇到 “怎么选 Q/R”“怎么保证矩阵对称”等常见难题。可以留言交流，老实说，再看记住，Kalma 是最优的。但前提是你给它正确的数据和合适的参数。祝你编码愉快，

一下Kalma 滤波到底有什么特点？🤔

⚡️ 高效递归算法 - 在每一步仅需一次预测+一次更新就可以完成状态估计，无需存储历史数据。

🧠 最优性 - 在已知程序模型与高斯白噪声条件下它提供全局最优解。

📐 可处理多变量程序 - 不论是一维还是二维甚至更高阶，只要把状态写成向量、协方差写成矩阵即可统一处理。

🔄 灵活可 - 对于非线性问题可以升级为 EKF / UKF；对于未知模型可以结合自适应方法动态调节 Q/R。

👉 小结：Kalma 滤波是一个理论成熟、实现简洁、性能优秀但需要“好参数” 的工具箱。在硬件受限或实时要求高的嵌入式项目里它往往能帮你把原始传感器读数变成精准而平滑的状态估计，而不是靠经验式公式去“逼近”真实世界。

如果你正在开发一个需要融合加速度/陀螺仪/GPS 等多源信息的小型机器人或无人机。希望能快速获得稳定的姿态或轨迹，那么就让 Kalma 滤波来帮你吧！

💡提示在实际部署之前务必进行一次完整的参数标定实验，并利用交叉验证检查收敛性能。

祝你编码顺利 🚀💡