失眠网 > 【51单片机快速入门指南】4.3.2： MPU6050：一阶互补滤波二阶互补滤波和卡尔曼滤波

【51单片机快速入门指南】4.3.2： MPU6050：一阶互补滤波二阶互补滤波和卡尔曼滤波

时间：2019-03-15 15:28:33

源码

stdint.h见【51单片机快速入门指南】1：基础知识和工程创建

软件I2C程序见【51单片机快速入门指南】4：软件 I2C

串口部分见【51单片机快速入门指南】3.3：USART 串口通信

MPU6050.c、MPU6050.h见【51单片机快速入门指南】4.3： I2C读取MPU6050陀螺仪的原始数据

MPU6050_Filter.c

#include "MPU6050.h"#include <math.h>#include "./MPU6050/MPU6050_Filter.h"#define PI 3.141592653589793float Delta_t = 1;float GYRO_K = 1;#define First_Order_Filter_Tau 0.075float First_Order_k = 1;void MPU6050_Filter_Init(float loop_ms){Delta_t = loop_ms/1000.;First_Order_k = First_Order_Filter_Tau / (First_Order_Filter_Tau + Delta_t);switch((MPU_Read_Byte(MPU_GYRO_CFG_REG) >> 3) & 3){case 0:GYRO_K = 131;break;case 1:GYRO_K = 65.5;break;case 2:GYRO_K = 32.8;break;case 3:GYRO_K = 16.4;break;}}float First_Order_Filter_Calc(int16_t acc1, int16_t acc3, int16_t gyro2, float * angle2){*angle2 = First_Order_k * (*angle2 + (-gyro2 / GYRO_K) * Delta_t) + (1 - First_Order_k) * (atan2(acc1, acc3) * 180 / PI);return *angle2;} #define Second_Order_Filter_k 5float Second_Order_Filter_Calc(int16_t acc1, int16_t acc3, int16_t gyro2, Second_Order_Filter* filter){float angle_m = atan2(acc1, acc3) * 180 / PI;float gyro_m = -gyro2 / GYRO_K;float x1, x2;x1 = (angle_m - filter->angle) * Second_Order_Filter_k * Second_Order_Filter_k;filter->y = filter->y + x1 * Delta_t;x2 = filter->y + 2 * Second_Order_Filter_k * (angle_m - filter->angle) + gyro_m;filter->angle = filter->angle + x2 * Delta_t;return filter->angle;}#define Q_angle0.05#define Q_gyro0.0003#define R_angle 0.01float MPU_Kalman_Filter_Calc(int16_t acc1, int16_t acc3, int16_t gyro2, MPU_Kalman_Filter* filter){float newAngle = atan2(acc1, acc3) * 180 / PI;float newRate = -gyro2 / GYRO_K;float E;float K_0, K_1;float Angle_err_x;filter->angle += Delta_t * (newRate - filter->Q_bias_x);filter->P_00 += - Delta_t * (filter->P_10 + filter->P_01) + Q_angle * Delta_t;filter->P_01 += - Delta_t * filter->P_11;filter->P_10 += - Delta_t * filter->P_11;filter->P_11 += + Q_gyro * Delta_t;Angle_err_x = newAngle - filter->angle;E = filter->P_00 + R_angle;K_0 = filter->P_00 / E;K_1 = filter->P_10 / E;filter->angle += K_0 * Angle_err_x;filter->Q_bias_x += K_1 * Angle_err_x;filter->P_00 -= K_0 * filter->P_00;filter->P_01 -= K_0 * filter->P_01;filter->P_10 -= K_1 * filter->P_00;filter->P_11 -= K_1 * filter->P_01;return filter->angle;}

MPU6050_Filter.h

#ifndef MPU6050_Filter_H_#define MPU6050_Filter_H_typedef struct{float y;float angle;}Second_Order_Filter;typedef struct{float P_00, P_01, P_10, P_11;float Q_bias_x;float angle;}MPU_Kalman_Filter;void MPU6050_Filter_Init(float loop_ms);float First_Order_Filter_Calc(int16_t acc1, int16_t acc3, int16_t gyro2, float * angle2);float Second_Order_Filter_Calc(int16_t acc1, int16_t acc3, int16_t gyro2, Second_Order_Filter* filter);float MPU_Kalman_Filter_Calc(int16_t acc1, int16_t acc3, int16_t gyro2, MPU_Kalman_Filter* filter);#endif

使用方法

先调用MPU6050_Filter_Init(dt)，参数为一次循环的时间，单位为ms

再使用滤波函数。

测试程序

生成的程序较大，对于89C52，需要注释掉没用到的函数。

一阶互补滤波

#include <STC89C5xRC.H>#include "intrins.h"#include "stdint.h"#include "USART.h"#include "./MPU6050/MPU6050.h"#include "./MPU6050/MPU6050_Filter.h"void Delay1ms()//@11.0592MHz{unsigned char i, j;_nop_();i = 2;j = 199;do{while (--j);} while (--i);}void Delay_ms(int i){while(i--)Delay1ms();}void main(void){int16_t aacx,aacy,aacz;//加速度传感器原始数据int16_t gyrox,gyroy,gyroz;//陀螺仪原始数据float anglex = 0;float angley = 0;float anglez = 0;USART_Init(USART_MODE_1, Rx_ENABLE, STC_USART_Priority_Lowest, 11059200, 57600, DOUBLE_BAUD_ENABLE, USART_TIMER_1);MPU_Init(); MPU6050_Filter_Init(47);while(1){MPU_Get_Accelerometer(&aacx, &aacy, &aacz);//得到加速度传感器数据MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);//得到陀螺仪数据printf("%f, " , First_Order_Filter(aacy, aacz, gyrox, &anglex));printf("%f, " , First_Order_Filter(aacx, aacz, gyroy, &angley));printf("%f\r\n",First_Order_Filter(aacx, aacy, gyroz, &anglez));}}

效果

只看了俯仰和滚转

First_Order_Filter_Tau要根据需要调节，我这里取First_Order_Filter_Tau = 0.075

二阶互补滤波

#include <STC89C5xRC.H>#include "intrins.h"#include "stdint.h"#include "USART.h"#include "./MPU6050/MPU6050.h"#include "./MPU6050/MPU6050_Filter.h"void Delay1ms()//@11.0592MHz{unsigned char i, j;_nop_();i = 2;j = 199;do{while (--j);} while (--i);}void Delay_ms(int i){while(i--)Delay1ms();}Second_Order_Filter anglex = {0, 0}, angley = {0, 0}, anglez = {0, 0};void main(void){int16_t aacx,aacy,aacz;//加速度传感器原始数据int16_t gyrox,gyroy,gyroz;//陀螺仪原始数据USART_Init(USART_MODE_1, Rx_ENABLE, STC_USART_Priority_Lowest, 11059200, 57600, DOUBLE_BAUD_ENABLE, USART_TIMER_1);MPU_Init(); MPU6050_Filter_Init(56);while(1){MPU_Get_Accelerometer(&aacx, &aacy, &aacz);//得到加速度传感器数据MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);//得到陀螺仪数据printf("%f, " , Second_Order_Filter_Calc(aacy, aacz, gyrox, &anglex));printf("%f, " , Second_Order_Filter_Calc(aacx, aacz, gyroy, &angley));printf("%f\r\n",Second_Order_Filter_Calc(aacx, aacy, gyroz, &anglez));}}

效果

只看了俯仰和滚转

Second_Order_Filter_k根据需要，越大跟随越快，越小越平滑

（我参考的大佬有取0.8的，有取10的，我这里取5）。

要根据需要调节。

卡尔曼滤波

#include <STC89C5xRC.H>#include "intrins.h"#include "stdint.h"#include "USART.h"#include "./MPU6050/MPU6050.h"#include "./MPU6050/MPU6050_Filter.h"void Delay1ms()//@11.0592MHz{unsigned char i, j;_nop_();i = 2;j = 199;do{while (--j);} while (--i);}void Delay_ms(int i){while(i--)Delay1ms();}MPU_Kalman_Filter anglex = {0};MPU_Kalman_Filter angley = {0};MPU_Kalman_Filter anglez = {0};void main(void){int16_t aacx,aacy,aacz;//加速度传感器原始数据int16_t gyrox,gyroy,gyroz;//陀螺仪原始数据USART_Init(USART_MODE_1, Rx_ENABLE, STC_USART_Priority_Lowest, 11059200, 57600, DOUBLE_BAUD_ENABLE, USART_TIMER_1);MPU_Init(); MPU6050_Filter_Init(76);while(1){MPU_Get_Accelerometer(&aacx, &aacy, &aacz);//得到加速度传感器数据MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);//得到陀螺仪数据printf("%f, " , MPU_Kalman_Filter_Calc(aacy, aacz, gyrox, &anglex));printf("%f, " , MPU_Kalman_Filter_Calc(aacx, aacz, gyroy, &angley));printf("%f\r\n",MPU_Kalman_Filter_Calc(aacx, aacy, gyroz, &anglez));}}