第 5 篇：卡尔曼滤波（下）——50 行 C 代码 + MPU6050 实战
上篇把原理讲透了。这篇直接上代码——一维卡尔曼、二维卡尔曼、MPU6050 角度估计，全都有。
1. 一维卡尔曼（不到 50 行）
// kalman1d.htypedefstruct{floatx;// 状态估计值floatp;// 估计误差协方差floatq;// 过程噪声协方差floatr;// 测量噪声协方差}kalman1d_t;voidkalman1d_init(kalman1d_t*kf,floatq,floatr,floatinit_x);floatkalman1d_update(kalman1d_t*kf,floatmeasurement);// kalman1d.c#include"kalman1d.h"voidkalman1d_init(kalman1d_t*kf,floatq,floatr,floatinit_x){kf->x=init_x;kf->p=1.0f;// 初始不确定性，设为 1（会快速收敛）kf->q=q;// 过程噪声kf->r=r;// 测量噪声}floatkalman1d_update(kalman1d_t*kf,floatz){// === 预测 ===// x = x（一维无控制输入，状态不变）kf->p=kf->p+kf->q;// P⁻ = P + Q// === 更新 ===floatk=kf->p/(kf->p+kf->r);// K = P⁻/(P⁻+R)kf->x=kf->x+k*(z-kf->x);// x̂ = x̂⁻ + K(z - x̂⁻)kf->p=(1.0f-k)*kf->p;// P = (1-K)P⁻returnkf->x;}使用示例——超声波测距：
kalman1d_tdist_kf;voidsetup(void){// R = 测距噪声方差。超声波测距精度约 ±2cm，方差 ≈ 4// Q = 目标不会瞬间移动，设小一点kalman1d_init(&dist_kf,0.01f,4.0f,100.0f);}voidloop(void){floatraw_dist=ultrasonic_read_cm();// 超声波原始距离floatopt_dist=kalman1d_update(&dist_kf,raw_dist);printf("raw=%.1fcm, kalman=%.1fcm\r\n",raw_dist,opt_dist);delay_ms(50);}2. 二维卡尔曼——同时估计位置和速度
很多场景下，我们不仅想知道"现在在哪"，还想知道"现在多快"。
状态向量 x = [位置, 速度]ᵀ
// kalman2d.htypedefstruct{floatx[2];// [位置, 速度]floatp[2][2];// 2×2 协方差矩阵floatq[2][2];// 过程噪声floatr;// 测量噪声（只测位置）floatdt;// 采样间隔}kalman2d_t;voidkalman2d_init(kalman2d_t*kf,floatdt,floatq_pos,floatq_vel,floatr);floatkalman2d_update(kalman2d_t*kf,floatmeasurement);voidkalman2d_get_state(kalman2d_t*kf,float*pos,float*vel);// kalman2d.c#include"kalman2d.h"#include<string.h>voidkalman2d_init(kalman2d_t*kf,floatdt,floatq_pos,floatq_vel,floatr){kf->dt=dt;kf->r=r;memset(kf->x,0,sizeof(kf->x));memset(kf->p,0,sizeof(kf->p));kf->p[0][0]=1.0f;kf->p[1][1]=1.0f;// 初始协方差kf->q[0][0]=q_pos;kf->q[1][1]=q_vel;// 对角噪声阵}floatkalman2d_update(kalman2d_t*kf,floatz){floatdt=kf->dt;// === 预测 ===// x⁻[0] = x[0] + x[1]*dt → 位置 = 位置 + 速度×时间// x⁻[1] = x[1] → 速度不变（匀速模型）floatx_pred[2];x_pred[0]=kf->x[0]+kf->x[1]*dt;x_pred[1]=kf->x[1];// P⁻ = A·P·Aᵀ + Q// A = [[1, dt], [0, 1]]floatp_pred[2][2];p_pred[0][0]=kf->p[0][0]+2*dt*kf->p[0][1]+dt*dt*kf->p[1][1]+kf->q[0][0];p_pred[0][1]=kf->p[0][1]+dt*kf->p[1][1];p_pred[1][0]=p_pred[0][1];p_pred[1][1]=kf->p[1][1]+kf->q[1][1];// === 更新（只测量位置 H=[1,0]）===floats=p_pred[0][0]+kf->r;// 新息协方差floatk0=p_pred[0][0]/s;// 卡尔曼增益 K[0]floatk1=p_pred[1][0]/s;// 卡尔曼增益 K[1]floaty=z-x_pred[0];// 新息 = 测量值 - 预测值kf->x[0]=x_pred[0]+k0*y;// 更新位置kf->x[1]=x_pred[1]+k1*y;// 更新速度kf->p[0][0]=(1-k0)*p_pred[0][0];kf->p[0][1]=(1-k0)*p_pred[0][1];kf->p[1][0]=p_pred[1][0]-k1*p_pred[0][0];kf->p[1][1]=p_pred[1][1]-k1*p_pred[0][1];returnkf->x[0];}voidkalman2d_get_state(kalman2d_t*kf,float*pos,float*vel){*pos=kf->x[0];*vel=kf->x[1];}3. 实战：MPU6050 角度卡尔曼滤波
用二维卡尔曼估计俯仰角——同时得到角度和角速度。
kalman2d_tangle_kf;voidmpu6050_angle_init(void){// dt = 5ms (200Hz 采样)// q_pos = 0.001 (角度过程噪声小)// q_vel = 0.003 (角速度噪声稍大)// r = 0.03 (加速度计推算角度的噪声，实际测出的方差)kalman2d_init(&angle_kf,0.005f,0.001f,0.003f,0.03f);}floatmpu6050_get_angle(void){floatax,ay,az,gx,gy,gz;mpu6050_read_all(&ax,&ay,&az,&gx,&gy,&gz);// 加速度计推算的角度作为"测量值"floataccel_angle=atan2f(ay,az)*180.0f/3.14159f;// 卡尔曼融合floatangle=kalman2d_update(&angle_kf,accel_angle);// 注意：这里把角速度信息用在了预测模型里（匀速模型）// 更精确的做法是把 gyro 作为控制输入 u[k] 放进预测方程returnangle;}效果对比：
纯加速度计: ±3° 晃动（高频振动） 纯陀螺仪: 持续漂移（1分钟漂5°） 互补滤波: ±1° 卡尔曼: ±0.5°4. 卡尔曼滤波的坑
坑 1：Q 和 R 的初始值选错导致发散
→ 解决办法：先用传感器数据手册算 R，Q 从 0.001 开始调
坑 2：协方差矩阵不对称导致数值不稳定
→ 解决办法：强制对称P[0][1] = P[1][0] = (P[0][1]+P[1][0])/2
坑 3：非线性系统用线性卡尔曼
→ 线性卡尔曼假设 A·x+B·u，如果你的系统不是线性的（比如四旋翼姿态），需要用扩展卡尔曼(EKF)
下一篇：互补滤波——陀螺仪+加速度计的最佳拍档，比卡尔曼更简单实用
