嵌入式常用滤波算法与控制算法（5）卡尔曼滤波（下）

第 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#includekalman1d.hvoidkalman1d_init(kalman1d_t*kf,floatq,floatr,floatinit_x){kf-xinit_x;kf-p1.0f;// 初始不确定性设为 1会快速收敛kf-qq;// 过程噪声kf-rr;// 测量噪声}floatkalman1d_update(kalman1d_t*kf,floatz){// 预测 // x x一维无控制输入状态不变kf-pkf-pkf-q;// P⁻ P Q// 更新 floatkkf-p/(kf-pkf-r);// K P⁻/(P⁻R)kf-xkf-xk*(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_distultrasonic_read_cm();// 超声波原始距离floatopt_distkalman1d_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#includekalman2d.h#includestring.hvoidkalman2d_init(kalman2d_t*kf,floatdt,floatq_pos,floatq_vel,floatr){kf-dtdt;kf-rr;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){floatdtkf-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]floatsp_pred[0][0]kf-r;// 新息协方差floatk0p_pred[0][0]/s;// 卡尔曼增益 K[0]floatk1p_pred[1][0]/s;// 卡尔曼增益 K[1]floatyz-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){*poskf-x[0];*velkf-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_angleatan2f(ay,az)*180.0f/3.14159f;// 卡尔曼融合floatanglekalman2d_update(angle_kf,accel_angle);// 注意这里把角速度信息用在了预测模型里匀速模型// 更精确的做法是把 gyro 作为控制输入 u[k] 放进预测方程returnangle;}效果对比纯加速度计: ±3° 晃动高频振动 纯陀螺仪: 持续漂移1分钟漂5° 互补滤波: ±1° 卡尔曼: ±0.5°4. 卡尔曼滤波的坑坑 1Q 和 R 的初始值选错导致发散→ 解决办法先用传感器数据手册算 RQ 从 0.001 开始调坑 2协方差矩阵不对称导致数值不稳定→ 解决办法强制对称P[0][1] P[1][0] (P[0][1]P[1][0])/2坑 3非线性系统用线性卡尔曼→ 线性卡尔曼假设 A·xB·u如果你的系统不是线性的比如四旋翼姿态需要用扩展卡尔曼(EKF)下一篇互补滤波——陀螺仪加速度计的最佳拍档比卡尔曼更简单实用