当前位置:   article > 正文

卡尔曼滤波融合库函数+Arduino实例_arduino卡尔曼函数库

arduino卡尔曼函数库

-------这篇文章就作为放弃ACM比赛转行到电子设计大赛的开始吧,ACM比赛真的太需要时间了,准确的说对于我这样的菜鸟而言太浪费时间了,但是话说回来两年时间从中真心收获了很多


        我是不理解卡尔曼滤波的原理啊,但是用这个库函数做个平衡车是绝对没问题 ,所以不理解没太大问题,只要知道它是用来融合加速度计 和 陀螺仪测定角度的。这个角度相对单纯求得的角度会更加精确,既然我弄不明白滤波的原理,下面我会特别详细的说明一下此库函数用到的变量,毕竟有很多人还是想弄明白的。

Q_angle:相对于加速度计的噪音协方差。    Q_bias:相对于陀螺仪的噪音协方差。  

R_measure:测量噪声协方差(有的版本是R_angle说是ACC的协方差)。  

***********************************以上私有变量使用者可以根据具体情况稍作修改,并且库提供set的函数***********************************


angle:最终要算得的角度。  bias:最终算得的陀螺零偏。  rate:最终算得的角速度。 P[2][2]:协方差误差矩阵。


************************************************************再说一下getAngle函数的参数*******************************************************

newAngle:这是去除了LBS偏差的角度单位是度。   newRate:去除了LBS偏差的角速度单位是度/秒。 dt:增量时间采样频率

**********************************************Kalman.h文件 当时找到没法复制 我就手打 所以忘记了作者***********************************

  1. #ifndef _Kalman_h_
  2. #define _Kalman_h_
  3. class Kalman {
  4. public:
  5. Kalman();
  6. float getAngle(float newAngle, float newRate, float dt);
  7. void setAngle(float angle);
  8. float getRate();
  9. void setQangle(float Q_angle);
  10. void setQbias(float Q_bias);
  11. void setRmeasure(float R_measure);
  12. float getQangle();
  13. float getQbias();
  14. float getRmeasure();
  15. private:
  16. float Q_angle;
  17. float Q_bias;
  18. float R_measure;
  19. float angle;
  20. float bias;
  21. float rate;
  22. float P[2][2];
  23. };
  24. #endif

*******************************************Kalman.cpp文件 当时找到没法复制 我就手打 所以忘记了作者**********************************

  1. #include "Kalman.h"
  2. Kalman::Kalman() {
  3. Q_angle = 0.001f;
  4. Q_bias = 0.003f;
  5. R_measure = 0.03f;
  6. angle = 0.0f;
  7. bias = 0.0f;
  8. P[0][0] = 0.0f;
  9. P[0][1] = 0.0f;
  10. P[1][0] = 0.0f;
  11. P[1][1] = 0.0f;
  12. };
  13. float Kalman::getAngle(float newAngle, float newRate, float dt) {
  14. rate = newRate - bias;
  15. angle += dt * rate;
  16. P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
  17. P[0][1] -= dt * P[1][1];
  18. P[1][0] -= dt * P[1][1];
  19. P[1][1] += Q_bias * dt;
  20. float S = P[0][0] + R_measure;
  21. float K[2];
  22. K[0] = P[0][0] / S;
  23. K[1] = P[1][0] / S;
  24. float y = newAngle - angle;
  25. angle += K[0] * y;
  26. bias += K[1] * y;
  27. float P00_temp = P[0][0];
  28. float P01_temp = P[0][1];
  29. P[0][0] -= K[0] * P00_temp;
  30. P[0][1] -= K[0] * P01_temp;
  31. P[1][0] -= K[1] * P00_temp;
  32. P[1][1] -= K[1] * P01_temp;
  33. return angle;
  34. };
  35. void Kalman::setAngle(float angle) { this->angle = angle; };
  36. float Kalman::getRate() { return this->rate; };
  37. void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; };
  38. void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; };
  39. void Kalman::setRmeasure(float R_measure) { this->R_measure = R_measure; };
  40. float Kalman::getQangle() { return this->Q_angle; };
  41. float Kalman::getQbias() { return this->Q_bias; };
  42. float Kalman::getRmeasure() { return this->R_measure; };

再附上一个Arduino示例程序用来观察自己mpu6050波形情况(因为每个人的mpu6050不一样可将各协方差稍作调整)

  1. /*
  2. title : Kalman fusion angle calculation
  3. date : 08/08/2015
  4. author : houwei
  5. */
  6. #include "Wire.h"
  7. #include "I2Cdev.h"
  8. #include "MPU6050.h"
  9. #include "Kalman.h"
  10. #define AX_ZERO -1181 //加速度计的0偏修正值
  11. #define GX_ZERO -176.85 //陀螺仪的0偏修正
  12. Kalman angle_feng;
  13. MPU6050 accelgyro;
  14. int16_t ax, ay, az;
  15. int16_t gx, gy, gz;
  16. double total_angle = 0;
  17. bool blinkState = false;
  18. void setup() {
  19. Wire.begin();
  20. Serial.begin(38400);
  21. Serial.println("Initializing I2C devices...");
  22. accelgyro.initialize();
  23. Serial.println("Testing device connections...");
  24. Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  25. }
  26. float Angle = 0.0; //卡尔曼融合最终角度
  27. unsigned long pretime = 0.0;//相当于执行的起始时间
  28. void loop()
  29. {
  30. double ax_angle = 0.0; //加速度计算得的角度
  31. double gx_angle = 0.0; //微分的每次角速度算得的角度
  32. double totgx_angle = 0.0; //总的角速度算得的角度
  33. unsigned long time = 0; //每执行一次loop所用时间
  34. unsigned long midtime = 0; //相当于执行的结束时间
  35. float gyro = 0.0, dt = 0.0;
  36. if (pretime == 0) pretime = millis();
  37. midtime = millis();
  38. accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  39. ax = ax - AX_ZERO;
  40. /*
  41. initialize()中加速的范围设置为2g
  42. 分辨率:16384 LSB/g
  43. sin(jiaodu) = k(xishu = 0.92) * 3.14 * (jiaodu) / 180
  44. = (jiasudu) / 16384 (jiasudu/16384 加速度
  45. 对应范围的实际值)
  46. */
  47. ax_angle = ax/263;
  48. /*
  49. initialize()中加速的范围设置为250度/s
  50. 分辨率:131 LSB/S
  51. gx_angle = ((gy/131)*dt)/1000
  52. totgx_angle += gx_angle
  53. */
  54. gy -= GX_ZERO;
  55. time = midtime - pretime;
  56. gyro = gy/131.0;
  57. gx_angle = gyro * time;
  58. gx_angle /= 1000.0;
  59. total_angle += gx_angle;
  60. dt = time / 1000.0;
  61. Angle = angle_feng.getAngle(ax_angle, gyro, dt);
  62. delay(1000);
  63. Serial.print(ax_angle);Serial.print(", ");
  64. Serial.print(total_angle); Serial.print(", ");
  65. Serial.print(Angle);
  66. pretime = midtime;
  67. }

上面有两个initialize()设置问题一个是加速度计-2g~+2g,对应16384 LSB/g  另一个陀螺仪-250度/s~+250度/s 对应131LSB/s有个图




声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/盐析白兔/article/detail/454872
推荐阅读
相关标签
  

闽ICP备14008679号