当前位置:   article > 正文

mpu6050卡尔曼滤波C语言代码_micropython mpu6050 卡尔曼

micropython mpu6050 卡尔曼

写在前面:目前网上很多卡尔曼滤波的c语言代码有一些小问题,且不方便移植,写这个博客的目前是想提供一个直接复制粘贴就能使用的c语言代码,理论推导部分请参考别的博客。

结构体定义

  1. typedef struct
  2. {
  3. float dt; //采样时间
  4. float angle_f; //角度滤波后
  5. float angle_m; //角度测量
  6. float wb_m; //角速度测量
  7. float wb_f; //角速度滤波后
  8. float q_bias; //角速度offset
  9. float P[2][2]; //协方差矩阵
  10. float Q_angle; // Q矩阵
  11. float Q_gyro;
  12. float R_angle; // R矩阵
  13. } Kalman_pm_st;

结构体初始化(赋值)

  1. void Kalman_Init(Kalman_pm_st* Kalman_pm)
  2. {
  3. /*注意:只需要调节Q矩阵[Q_angle,Q_gyro]和R矩阵[R_angle]即可*/
  4. Kalman_pm->P[0][0] = 1.0f;
  5. Kalman_pm->P[0][1] = 0.0f;
  6. Kalman_pm->P[1][0] = 0.0f;
  7. Kalman_pm->P[1][1] = 1.0f;
  8. Kalman_pm->dt = 0.001f;
  9. Kalman_pm->Q_angle = 0.001f;
  10. Kalman_pm->Q_gyro = 0.003f;
  11. Kalman_pm->R_angle = 0.5f;
  12. Kalman_pm->q_bias = 0.0f;
  13. Kalman_pm->angle_f = 0.0f;
  14. }

卡尔曼滤波函数

  1. void Kalman_Filter(Kalman_pm_st *kalman_pm)
  2. {
  3. float angle_err; //先验误差
  4. float angle_; //先验估计
  5. float Pdot[2][2]; //先验误差协方差矩阵
  6. float K_0;
  7. float K_1; //卡尔曼增益
  8. /*先验估计*/
  9. angle_ = kalman_pm->angle_f + (kalman_pm->wb_m - kalman_pm->q_bias) * kalman_pm->dt; //先验估计
  10. angle_err = kalman_pm->angle_m - angle_; //先验误差
  11. /*先验误差协方差矩阵*/
  12. Pdot[0][0] = kalman_pm->P[0][0] + kalman_pm->Q_angle - (kalman_pm->P[0][1] + kalman_pm->P[1][0]) * kalman_pm->dt; // Q_angle->Q1
  13. Pdot[0][1] = kalman_pm->P[0][1] - (kalman_pm->P[1][1]) * kalman_pm->dt;
  14. Pdot[1][0] = kalman_pm->P[1][0] - (kalman_pm->P[1][1]) * kalman_pm->dt;
  15. Pdot[1][1] = kalman_pm->P[1][1] + kalman_pm->Q_gyro; // Q_gyro->Q2
  16. /*卡尔曼增益*/
  17. K_0 = Pdot[0][0] / (Pdot[0][0] + kalman_pm->R_angle);
  18. K_1 = Pdot[1][0] / (Pdot[0][0] + kalman_pm->R_angle);
  19. /*后验估计*/
  20. kalman_pm->angle_f = angle_ + K_0 * angle_err; //最优角度
  21. kalman_pm->q_bias += K_1 * angle_err; //最优角速度偏差
  22. kalman_pm->wb_f = kalman_pm->wb_m - kalman_pm->q_bias; //最优角速度
  23. /*更新误差协方差矩阵*/
  24. kalman_pm->P[0][0] = Pdot[0][0] - K_0 * Pdot[0][0];
  25. kalman_pm->P[0][1] = Pdot[0][1] - K_0 * Pdot[0][1];
  26. kalman_pm->P[1][0] = Pdot[1][0] - K_1 * Pdot[0][0];
  27. kalman_pm->P[1][1] = Pdot[1][1] - K_1 * Pdot[0][1];
  28. }

使用方法:

  1. Kalman_pm_st Kalman_pm; //定义结构体
  2. Kalman_Init(&Kalman_pm); //给结构体赋初值和修改参数
  3. /*参数输入*/
  4. Kalman_pm.angle_m=yaw_acc; //加速度计求解角度的值(偏航角)
  5. Kalman_pm.wb_m=wz_gyro; //陀螺仪的角速度(偏航角速度)
  6. Kalman_Filter(&Kalman_pm); //调用卡尔曼滤波
  7. float yaw_angle=Kalman_pm.angle_f; //滤波后的角度
  8. float yaw_w=Kalman_pm.wb_f; //滤波后的角速度

声明:本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:【wpsshop博客】
推荐阅读
相关标签
  

闽ICP备14008679号