目錄

• 源碼
• • MPU6050_Filter.c
  • MPU6050_Filter.h
• 使用方法
• 測試程序
• • 一階互補濾波
  • • 效果
  • 二階互補濾波
  • • 效果
  • 卡爾曼濾波
  • • 效果
• 總結(jié)

普中51-單核-A2
STC89C52
Keil uVision V5.29.0.0
PK51 Prof.Developers Kit Version:9.60.0.0
上位機：Vofa+ 1.3.10

---

參考資料：
MPU6050數(shù)據(jù)采集及其意義和濾波（一階互補濾波、二階互補濾波、卡爾曼濾波）—— 275891381
關(guān)于MPU6050姿態(tài)解算的一階互補濾波方法（從原理到代碼實現(xiàn)） —— 可以叫我馬同學(xué)
姿態(tài)融合的一階互補濾波、二階互補濾波、卡爾曼濾波核心程序 —— 賣硬件的

源碼

???????stdint.h見【51單片機快速入門指南】1：基礎(chǔ)知識和工程創(chuàng)建
???????軟件I2C程序見【51單片機快速入門指南】4： 軟件 I2C
???????串口部分見【51單片機快速入門指南】3.3：USART 串口通信
???????MPU6050.c、MPU6050.h見【51單片機快速入門指南】4.3： I2C讀取MPU6050陀螺儀的原始數(shù)據(jù)

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.075 float 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_angle 0.05 #define Q_gyro 0.0003 #define R_angle 0.01 float 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

使用方法

???????先調(diào)用MPU6050_Filter_Init(dt)，參數(shù)為一次循環(huán)的時間，單位為ms
???????再使用濾波函數(shù)。

測試程序

???????生成的程序較大，對于89C52，需要注釋掉沒用到的函數(shù)。

一階互補濾波

#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; //加速度傳感器原始數(shù)據(jù)int16_t gyrox,gyroy,gyroz; //陀螺儀原始數(shù)據(jù)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); //得到加速度傳感器數(shù)據(jù)MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz); //得到陀螺儀數(shù)據(jù)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));} }

效果

???????只看了俯仰和滾轉(zhuǎn)
???????First_Order_Filter_Tau 要根據(jù)需要調(diào)節(jié)，我這里取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; //加速度傳感器原始數(shù)據(jù)int16_t gyrox,gyroy,gyroz; //陀螺儀原始數(shù)據(jù)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); //得到加速度傳感器數(shù)據(jù)MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz); //得到陀螺儀數(shù)據(jù)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));} }

效果

???????只看了俯仰和滾轉(zhuǎn)
???????Second_Order_Filter_k根據(jù)需要，越大跟隨越快，越小越平滑
???????（我參考的大佬有取0.8的，有取10的，我這里取5）。
???????要根據(jù)需要調(diào)節(jié)。

卡爾曼濾波

#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; //加速度傳感器原始數(shù)據(jù)int16_t gyrox,gyroy,gyroz; //陀螺儀原始數(shù)據(jù)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); //得到加速度傳感器數(shù)據(jù)MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz); //得到陀螺儀數(shù)據(jù)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));} }

效果

???????只看了俯仰和滾轉(zhuǎn)
???????Q參數(shù):過程噪聲協(xié)方差 Q參數(shù)調(diào)濾波后的曲線平滑程度，Q越小越平滑;
???????R參數(shù):觀測噪聲協(xié)方差 R參數(shù)調(diào)整濾波后的曲線與實測曲線的相近程度，R越小越接近(收斂越快)
???????我參考的大佬有取0.01，0.0003，0.01的，也有取0.001，0.005，0.5的
???????我這里取
???????Q_angle=0.05
???????Q_gyro=0.0003
???????R_angle=0.01
???????要根據(jù)需要調(diào)節(jié)。

在suhetao/stm32f4_mpu9250中有大神對EKF / UKF / CKF / SRCKF的實現(xiàn)，感興趣的可以看看。

總結(jié)

???????由于每種濾波器的參數(shù)都會極大地影響該濾波器的性能（一階濾波、二階濾波各一個參數(shù)，卡爾曼濾波三個參數(shù)），因此難以互相比較，我建議根據(jù)單片機的資源、性能選擇要用的濾波器，調(diào)參時配合上位機觀察立方體的效果和對應(yīng)波形。

總結(jié)

以上是生活随笔為你收集整理的【51单片机快速入门指南】4.3.2： MPU6050：一阶互补滤波、二阶互补滤波和卡尔曼滤波获取欧拉角的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。