失眠网 > MPU6050开发 -- 卡尔曼滤波

MPU6050开发 -- 卡尔曼滤波

时间：2023-06-19 18:28:52

相关推荐

MPU6050开发 -- 卡尔曼滤波

如需转载请注明出处：/qq_29350001/article/details/78687974

上一篇文章有讲到卡尔曼滤波了，现在需要将其添加到我们之前的C52测试程序中。

STM32 相关工程，下载：STM32F10x 卡尔曼滤波

一、再看一下卡尔曼滤波程序

#include<math.h> #include "stm32f10x.h" #include "Kalman_Filter.h" //卡尔曼滤波参数与函数 float dt=0.001;//注意：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; //卡尔曼滤波 float 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;//最优角速度 return angle； }

或者：

#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;//最优角速度 }

二、解析

卡尔曼滤波函数有两个参数，它的实参定义为angleAx，gyroGy;//计算后的角度（与x轴夹角）和角速度

那么如何计算angleAx，gyroGy？

（1）angleAx 计算

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

ax、az是什么？

MPU6050初始化设置范围为2g时，灵敏度 16384 LSB/g

ax =ACCEL_XOUT_H /16384

az =ACCEL_ZOUT_H /16384

因此可以这样写：

angleAx=atan2(（ACCEL_XOUT_H /16384）,（ACCEL_ZOUT_H /16384）)*180/3.14;

（2）gyroGy 计算

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

注意，131.00 是当陀螺仪量程为± 250 °/s时的灵敏度 131LSB/°/s。

MPU6050初始化设置范围为± 2000 °/s时，灵敏度为 16.4LSB/°/s。

gy是什么？

gy =GYRO_YOUT_H

因此可以这样写：

gyroGy=-GYRO_YOUT_H/16.4;

（3）卡尔曼滤波函数

而这样的一个 Kalman_Filter(angleAx, gyroGy); 卡尔曼滤波，每次卡只能得到一个方向的角度。

如此说来，我们再获取其他两个角度即可。

具体上面这三个与姿态角（Euler角）yaw pitch roll 对应关系，我还没搞清楚...

但大体上卡尔曼滤波和C52测试程序就是这样子结合的。只待进一步验证了...

三、编写程序

参看：MPU6050 卡尔曼滤波

//**************************************** // Update to MPU6050 by shinetop // MCU: STC89C52 // .3.1 // 功能: 显示加速度计和陀螺仪的10位原始数据 //**************************************** // 使用单片机STC89C52 // 晶振：11.0592M // 显示：串口 // 编译环境 Keil uVision2 //**************************************** #include <REG52.H>#include <math.h> //Keil library #include <stdio.h> //Keil library#include <INTRINS.H> typedef unsigned char uchar; typedef unsigned short ushort; typedef unsigned int uint; //**************************************** // 定义51单片机端口 //**************************************** sbit SCL=P1^5; //IIC时钟引脚定义 sbit SDA=P1^4; //IIC数据引脚定义 //**************************************** // 定义MPU6050内部地址 //**************************************** #define SMPLRT_DIV0x19 //陀螺仪采样率，典型值：0x07(125Hz) #define CONFIG0x1A //低通滤波频率，典型值：0x06(5Hz) #define GYRO_CONFIG0x1B //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s) #define ACCEL_CONFIG 0x1C //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz) #define ACCEL_XOUT_H 0x3B #define ACCEL_XOUT_L 0x3C #define ACCEL_YOUT_H 0x3D #define ACCEL_YOUT_L 0x3E #define ACCEL_ZOUT_H 0x3F #define ACCEL_ZOUT_L 0x40 #define TEMP_OUT_H0x41 #define TEMP_OUT_L0x42 #define GYRO_XOUT_H0x43 #define GYRO_XOUT_L0x44#define GYRO_YOUT_H0x45 #define GYRO_YOUT_L0x46 #define GYRO_ZOUT_H0x47 #define GYRO_ZOUT_L0x48 #define PWR_MGMT_10x6B //电源管理，典型值：0x00(正常启用) #define WHO_AM_I 0x75 //IIC地址寄存器(默认数值0x68，只读) #define SlaveAddress 0xD0 //IIC写入时的地址字节数据，+1为读取 //************************************************************************************************** //定义类型及变量 //************************************************************************************************** uchar dis[6]; //显示数字(-511至512)的字符数组 int dis_data; //变量 //******角度参数************ float Gyro_y; //Y轴陀螺仪数据暂存 float Angle_gy;//由角速度计算的倾斜角度 float Accel_x; //X轴加速度值暂存 float Angle_ax;//由加速度计算的倾斜角度 float Angle; //小车最终倾斜角度 uchar value; //角度正负极性标记 //************************************************************************************************** //函数声明 //************************************************************************************************** void Delay5us(); void delay(unsigned int k);//延时void lcd_printf(uchar *s,int temp_data); //********************************MPU6050操作函数*************************************************** void InitMPU6050(); //初始化MPU6050 void I2C_Start(); void I2C_Stop(); void I2C_SendACK(bit ack); bit I2C_RecvACK(); void I2C_SendByte(uchar dat); uchar I2C_RecvByte(); void I2C_ReadPage(); void I2C_WritePage(); void display_ACCEL_x(); void display_ACCEL_y(); void display_ACCEL_z(); uchar Single_ReadI2C(uchar REG_Address); //读取I2C数据 void Single_WriteI2C(uchar REG_Address,uchar REG_data); //向I2C写入数据 //******************************************************************************** //整数转字符串 //******************************************************************************** void lcd_printf(uchar *s,int temp_data) { if(temp_data<0) { temp_data=-temp_data; *s='-'; } else *s=' '; *++s =temp_data/10000+0x30; temp_data=temp_data%10000;//取余运算 *++s =temp_data/1000+0x30; temp_data=temp_data%1000;//取余运算 *++s =temp_data/100+0x30; temp_data=temp_data%100;//取余运算 *++s =temp_data/10+0x30; temp_data=temp_data%10;//取余运算 *++s =temp_data+0x30;} //****************************************************************************************************** //串口初始化 //******************************************************************************************************* void init_uart() { TMOD=0x21;TH1=0xfd; //实现波特率9600（系统时钟11.0592MHZ） TL1=0xfd; SCON=0x50; PS=1;//串口中断设为高优先级别 TR0=1;//启动定时器 TR1=1; ET0=1;//打开定时器0中断 ES=1;EA=1; } //************************************************************************************************* //串口发送函数 //************************************************************************************************* void SeriPushSend(uchar send_data) { SBUF=send_data; while(!TI);TI=0; } //************************************************************************************************* //************************************延时********************************************************* //************************************************************************************************* void delay(unsigned int k) {unsigned int i,j; for(i=0;i<k;i++) { for(j=0;j<121;j++); }} //************************************************************************************************ //延时5微秒(STC90C52RC@12M) //不同的工作环境,需要调整此函数 //注意当改用1T的MCU时,请调整此延时函数 //************************************************************************************************ void Delay5us() { _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); } //************************************************************************************************* //I2C起始信号 //************************************************************************************************* void I2C_Start() { SDA = 1;//拉高数据线 SCL = 1;//拉高时钟线 Delay5us(); //延时 SDA = 0;//产生下降沿 Delay5us(); //延时 SCL = 0;//拉低时钟线 } //************************************************************************************************* //I2C停止信号 //************************************************************************************************* void I2C_Stop() { SDA = 0;//拉低数据线 SCL = 1;//拉高时钟线 Delay5us(); //延时 SDA = 1;//产生上升沿 Delay5us(); //延时 } //************************************************************************************************** //I2C发送应答信号 //入口参数:ack (0:ACK 1:NAK) //************************************************************************************************** void I2C_SendACK(bit ack) { SDA = ack; //写应答信号 SCL = 1;//拉高时钟线 Delay5us(); //延时 SCL = 0;//拉低时钟线 Delay5us(); //延时 } //**************************************************************************************************** //I2C接收应答信号 //**************************************************************************************************** bit I2C_RecvACK() { SCL = 1;//拉高时钟线 Delay5us(); //延时 CY = SDA; //读应答信号 SCL = 0;//拉低时钟线 Delay5us(); //延时 return CY; } //***************************************************************************************************** //向I2C总线发送一个字节数据 //***************************************************************************************************** void I2C_SendByte(uchar dat) { uchar i; for (i=0; i<8; i++) //8位计数器 { dat <<= 1; //移出数据的最高位 SDA = CY;//送数据口 SCL = 1;//拉高时钟线 Delay5us(); //延时 SCL = 0;//拉低时钟线 Delay5us(); //延时 } I2C_RecvACK(); } //***************************************************************************************************** //从I2C总线接收一个字节数据 //****************************************************************************************************** uchar I2C_RecvByte() { uchar i; uchar dat = 0; SDA = 1;//使能内部上拉,准备读取数据, for (i=0; i<8; i++) //8位计数器 { dat <<= 1; SCL = 1;//拉高时钟线 Delay5us(); //延时 dat |= SDA; //读数据 SCL = 0;//拉低时钟线 Delay5us(); //延时 } return dat; } //***************************************************************************************************** //向I2C设备写入一个字节数据 //***************************************************************************************************** void Single_WriteI2C(uchar REG_Address,uchar REG_data) { I2C_Start(); //起始信号 I2C_SendByte(SlaveAddress); //发送设备地址+写信号 I2C_SendByte(REG_Address); //内部寄存器地址， I2C_SendByte(REG_data); //内部寄存器数据， I2C_Stop(); //发送停止信号 } //******************************************************************************************************* //从I2C设备读取一个字节数据 //******************************************************************************************************* uchar Single_ReadI2C(uchar REG_Address) { uchar REG_data; I2C_Start(); //起始信号 I2C_SendByte(SlaveAddress); //发送设备地址+写信号 I2C_SendByte(REG_Address);//发送存储单元地址，从0开始 I2C_Start(); //起始信号 I2C_SendByte(SlaveAddress+1); //发送设备地址+读信号 REG_data=I2C_RecvByte(); //读出寄存器数据 I2C_SendACK(1);//接收应答信号 I2C_Stop();//停止信号 return REG_data; } //****************************************************************************************************** //初始化MPU6050 //****************************************************************************************************** void InitMPU6050() { Single_WriteI2C(PWR_MGMT_1, 0x00); //解除休眠状态 Single_WriteI2C(SMPLRT_DIV, 0x07); Single_WriteI2C(CONFIG, 0x06); Single_WriteI2C(GYRO_CONFIG, 0x18); Single_WriteI2C(ACCEL_CONFIG, 0x01); } //****************************************************************************************************** //合成数据 //****************************************************************************************************** int GetData(uchar REG_Address) { uchar H,L; H=Single_ReadI2C(REG_Address); L=Single_ReadI2C(REG_Address+1); return ((H<<8)+L); //合成数据 } //****************************************************************************************************** //超级终端（串口调试助手）上显示10位数据 //****************************************************************************************************** void Display10BitData(int value) { uchar i; // value/=64;//转换为10位数据 lcd_printf(dis, value); //转换数据显示 for(i=0;i<6;i++) { SeriPushSend(dis[i]); } // DisplayListChar(x,y,dis,4); //启始列，行，显示数组，显示长度 } //********************************************************* // 卡尔曼滤波 //********************************************************* //Kalman滤波，20MHz的处理时间约0.77ms； float Kalman_Filter(float Accel,float Gyro) { static const float Q_angle=0.001; static const float Q_gyro=0.003;static const float R_angle=0.5;static const float dt=0.01; //dt为kalman滤波器采样时间;static const char C_0 = 1;static float Q_bias, Angle_err;static float PCt_0, PCt_1, E;static float K_0, K_1, t_0, t_1;static float Pdot[4] ={0,0,0,0};static float PP[2][2] = { { 1, 0 },{ 0, 1 } };Angle+=(Gyro - Q_bias) * dt; //先验估计 Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分 Pdot[1]=- PP[1][1]; Pdot[2]=- PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分 PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差 PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; //zk-先验估计 PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[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 * PP[0][1]; PP[0][0] -= K_0 * t_0; //后验估计误差协方差 PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle += K_0 * Angle_err; //后验估计 Q_bias += K_1 * Angle_err; //后验估计 Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度 return Gyro_y; } //********************************************************* // 倾角计算（卡尔曼融合） //********************************************************* void Angle_Calcu(void) { //------加速度-------------------------- //范围为2g时，换算关系：16384 LSB/g //角度较小时，x=sinx得到角度（弧度）, deg = rad*180/3.14 //因为x>=sinx,故乘以1.3适当放大 Accel_x = GetData(ACCEL_XOUT_H); //读取X轴加速度 Angle_ax = (Accel_x - 1100) /16384; //去除零点偏移,计算得到角度（弧度） Angle_ax = Angle_ax*1.4*180/3.14;//弧度转换为度, //Display10BitData(Angle_ax,2,1); //-------角速度------------------------- //范围为2000deg/s时，换算关系：16.4 LSB/(deg/s) Gyro_y = GetData(GYRO_YOUT_H); //静止时角速度Y轴输出为-30左右 Gyro_y = -(Gyro_y + 30)/16.4; //去除零点偏移，计算角速度值,负号为方向处理 //Angle_gy = Angle_gy + Gyro_y*0.01; //角速度积分得到倾斜角度. //Display10BitData(Gyro_y,8,1); //-------卡尔曼滤波融合----------------------- //Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角 Display10BitData(Kalman_Filter(Angle_ax,Gyro_y)); /*//-------互补滤波----------------------- //补偿原理是取当前倾角和加速度获得倾角差值进行放大，然后与 //陀螺仪角速度叠加后再积分，从而使倾角最跟踪为加速度获得的角度 //0.5为放大倍数，可调节补偿度；0.01为系统周期10ms Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/ } //******************************************************************************************************* //主程序 //******************************************************************************************************* void main() { delay(500);//上电延时 init_uart(); InitMPU6050(); //初始化MPU6050 delay(150); while(1) { Angle_Calcu(); SeriPushSend(0x0d); SeriPushSend(0x0a);//换行，回车 delay(2000); } }

我觉的这次代码问题不大了啊，为什么获取的值为 -00001 还是不对。

继续思考！！

四、可能出现错误：

Rebuild target 'Target 1'assembling STARTUP.A51...compiling MPU6050_C52.c...linking...*** WARNING L16: UNCALLED SEGMENT, IGNORED FOR OVERLAY PROCESSSEGMENT: ?PR?_DISPLAY10BITDATA?MPU6050_C52*** WARNING L16: UNCALLED SEGMENT, IGNORED FOR OVERLAY PROCESSSEGMENT: ?PR?_KALMAN_FILTER?MPU6050_C52*** WARNING L16: UNCALLED SEGMENT, IGNORED FOR OVERLAY PROCESSSEGMENT: ?PR?ANGLE_CALCULATE?MPU6050_C52*** ERROR L107: ADDRESS SPACE OVERFLOWSPACE: DATA SEGMENT: ?DT?MPU6050_C52LENGTH: 0071H*** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENTSYMBOL: GYRO_YSEGMENT: ?DT?MPU6050_C52*** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENTSYMBOL: ANGLESEGMENT: ?DT?MPU6050_C52*** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENTSYMBOL: DISSEGMENT: ?DT?MPU6050_C52*** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENTSYMBOL: DIS_DATASEGMENT: ?DT?MPU6050_C52*** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENTSYMBOL: ACCEL_XSEGMENT: ?DT?MPU6050_C52*** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENTSYMBOL: ANGLE_GYSEGMENT: ?DT?MPU6050_C52*** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENTSYMBOL: ANGLE_AXSEGMENT: ?DT?MPU6050_C52Program Size: data=135.1 xdata=0 code=2873Target not created

说明data空间已经不够用，原因是你可能有好多函数，而函数内部的局部变量又没有定义其空间。

这种情况下，系统会将变量分配到你在 Otions for Target 对话框里的设置的空间。

如果你在下图所示中的 Memory Model 里设置成 Small:variables in DATA,则DATA空间很快便用完，导致data空间不够用。

解决的办法有两种：

一是通过更改Memory Model设置，可以设置成pdata或xdata，以便有足够大的空间。

但这又带来新的问题，程序运行速度减慢，而且code代码也会加大，因为如果一个局部变量被存放在了xdata空间，汇编语言访问xdata空间的代码大小要比访问data空间的代码大，变量一旦很多，程序的代码也会逐渐增大；

二是根据自己的要求设置变量的空间。

所以这涉及到代码优化的问题，遇到具体问题时，在运行速度和代码大小之间取得适合自己的情况。

如需转载请注明出处：/qq_29350001/article/details/78687974

如果觉得《MPU6050开发 -- 卡尔曼滤波》对你有帮助，请点赞、收藏，并留下你的观点哦！

本内容不代表本网观点和政治立场，如有侵犯你的权益请联系我们处理。

网友评论