查看: 563|回复: 0
打印 上一主题 下一主题

MPU6050数据(一阶互补滤波/二阶互补/卡尔曼滤波)

[复制链接]
跳转到指定楼层
沙发
发表于 2016-3-19 17:41:51 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
1. MPU6050陀螺仪



// 陀螺仪

float angleAx,gyroGy;

MPU6050 accelgyro;

int16_t ax, ay, az, gx, gy, gz;



     accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//原始数据采集

     angleAx=atan2(ax,az)*180/PI;//加速度计算角度

     gyroGy=-gy/131.00;//陀螺仪角速度,注意正负号与放置有关





2.滤波参数及函数

//一阶互补滤波

float K1 =0.05; // 对加速度计取值的权重

float dt=20*0.001;//注意:dt的取值为滤波器采样时间

float angle1;



void Yijielvbo(float angle_m, float gyro_m)//采集后计算的角度和角加速度

{

      angle1 = K1 * angle_m+ (1-K1) * (angle1 + gyro_m * dt);

}



//二阶互补滤波

float K2 =0.2; // 对加速度计取值的权重

float x1,x2,y1;

float dt=20*0.001;//注意:dt的取值为滤波器采样时间

float angle2;



void Erjielvbo(float angle_m,float gyro_m)//采集后计算的角度和角加速度

{

     x1=(angle_m-angle2)*(1-K2)*(1-K2);

     y1=y1+x1*dt;

     x2=y1+2*(1-K2)*(angle_m-angle2)+gyro_m;

     angle2=angle2+ x2*dt;

}





//卡尔曼滤波参数与函数

float dt=0.005;//注意:dt的取值为kalman滤波器采样时间

float angle, angle_dot;//角度和角速度

float P[2][2] = {{ 1, 0 },

               { 0, 1 }};

float Pdot[4] ={ 0,0,0,0};

float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度

float R_angle=0.5 ,C_0 = 1;

float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;





//卡尔曼滤波

void Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy

{

angle+=(gyro_m-q_bias) * dt;

angle_err = angle_m - angle;

Pdot[0]=Q_angle - P[0][1] - P[1][0];

Pdot[1]=- P[1][1];

Pdot[2]=- P[1][1];

Pdot[3]=Q_gyro;

P[0][0] += Pdot[0] * dt;

P[0][1] += Pdot[1] * dt;

P[1][0] += Pdot[2] * dt;

P[1][1] += Pdot[3] * dt;

PCt_0 = C_0 * P[0][0];

PCt_1 = C_0 * P[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;

K_1 = PCt_1 / E;

t_0 = PCt_0;

t_1 = C_0 * P[0][1];

P[0][0] -= K_0 * t_0;

P[0][1] -= K_0 * t_1;

P[1][0] -= K_1 * t_0;

P[1][1] -= K_1 * t_1;

angle += K_0 * angle_err; //最优角度

q_bias += K_1 * angle_err;

angle_dot = gyro_m-q_bias;//最优角速度

}





3 三种滤波比较源代码 //











#include "Wire.h"

#include "I2Cdev.h"

#include "MPU6050.h"

#include "Timer.h"//时间操作系统头文件  本程序用作timeChange时间采集并处理一次数据



Timer t;//时间类



float timeChange=20;//滤波法采样时间间隔毫秒

float dt=timeChange*0.001;//注意:dt的取值为滤波器采样时间

// 陀螺仪

float angleAx,gyroGy;//计算后的角度(与x轴夹角)和角速度

MPU6050 accelgyro;//陀螺仪类

int16_t ax, ay, az, gx, gy, gz;//陀螺仪原始数据 3个加速度+3个角速度



//一阶滤波

float K1 =0.05; // 对加速度计取值的权重

//float dt=20*0.001;//注意:dt的取值为滤波器采样时间

float angle1;//一阶滤波角度输出

//二阶滤波

float K2 =0.2; // 对加速度计取值的权重

float x1,x2,y1;//运算中间变量

//float dt=20*0.001;//注意:dt的取值为滤波器采样时间

float angle2;//er阶滤波角度输出



//卡尔曼滤波参数与函数

float angle, angle_dot;//角度和角速度

float angle_0, angle_dot_0;//采集来的角度和角速度

//float dt=20*0.001;//注意:dt的取值为kalman滤波器采样时间

//一下为运算中间变量

float P[2][2] = {{ 1, 0 },

               { 0, 1 }};

float Pdot[4] ={ 0,0,0,0};

float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度

float R_angle=0.5 ,C_0 = 1;

float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;



void setup() {

     Wire.begin();//初始化

     Serial.begin(9600);//初始化

     accelgyro.initialize();//初始化



     int tickEvent1=t.every(timeChange, getangle);//本语句执行以后timeChange毫秒执行回调函数getangle



     int tickEvent2=t.every(50, printout) ;//本语句执行以后50毫秒执行回调函数printout,串口输出

}

void loop() {



     t.update();//时间操作系统运行



}

void printout()

{

      Serial.print(angleAx);Serial.print(',');

     Serial.print(angle1);Serial.print(',');

     Serial.print(angle2);Serial.print(',');

     // Serial.print(gx/131.00);Serial.print(',');

     Serial.println(angle);//Serial.print(',');



//   Serial.println(Output);

}





void getangle()

{

     accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取原始6个数据

     angleAx=atan2(ax,az)*180/PI;//计算与x轴夹角

     gyroGy=-gy/131.00;//计算角速度

     Yijielvbo(angleAx,gyroGy);//一阶滤波

     Erjielvbo(angleAx,gyroGy);//二阶滤波

     Kalman_Filter(angleAx,gyroGy);   //卡尔曼滤波

}







void Yijielvbo(float angle_m, float gyro_m)

{

     angle1 = K1 * angle_m+ (1-K1) * (angle1 + gyro_m * dt);

}



void Erjielvbo(float angle_m,float gyro_m)

{

     x1=(angle_m-angle2)*(1-K2)*(1-K2);

     y1=y1+x1*dt;

     x2=y1+2*(1-K2)*(angle_m-angle2)+gyro_m;

     angle2=angle2+ x2*dt;

}



void Kalman_Filter(double angle_m,double gyro_m)

{

angle+=(gyro_m-q_bias) * dt;

angle_err = angle_m - angle;

Pdot[0]=Q_angle - P[0][1] - P[1][0];

Pdot[1]=- P[1][1];

Pdot[2]=- P[1][1];

Pdot[3]=Q_gyro;

P[0][0] += Pdot[0] * dt;

P[0][1] += Pdot[1] * dt;

P[1][0] += Pdot[2] * dt;

P[1][1] += Pdot[3] * dt;

PCt_0 = C_0 * P[0][0];

PCt_1 = C_0 * P[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;

K_1 = PCt_1 / E;

t_0 = PCt_0;

t_1 = C_0 * P[0][1];

P[0][0] -= K_0 * t_0;

P[0][1] -= K_0 * t_1;

P[1][0] -= K_1 * t_0;

P[1][1] -= K_1 * t_1;

angle += K_0 * angle_err; //最优角度

q_bias += K_1 * angle_err;

angle_dot = gyro_m-q_bias;//最优角速度

}


本资料仅供学习使用
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 加入中科因仑

本版积分规则

快速回复 返回顶部 返回列表