参考资料：
MPU6050数据采集及其意义和滤波（一阶互补滤波、二阶互补滤波、卡尔曼滤波）—— 275891381
关于MPU6050姿态解算的一阶互补滤波方法（从原理到代码实现） —— 可以叫我马同学
姿态融合的一阶互补滤波、二阶互补滤波、卡尔曼滤波核心程序 —— 卖硬件的

源码

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.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

使用方法

先调用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));}
}

效果

只看了俯仰和滚转
       Q参数:过程噪声协方差 Q参数调滤波后的曲线平滑程度，Q越小越平滑;
       R参数:观测噪声协方差 R参数调整滤波后的曲线与实测曲线的相近程度，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
       要根据需要调节。

在suhetao/stm32f4_mpu9250中有大神对EKF / UKF / CKF / SRCKF的实现，感兴趣的可以看看。

总结

由于每种滤波器的参数都会极大地影响该滤波器的性能（一阶滤波、二阶滤波各一个参数，卡尔曼滤波三个参数），因此难以互相比较，我建议根据单片机的资源、性能选择要用的滤波器，调参时配合上位机观察立方体的效果和对应波形。

【51单片机快速入门指南】4.3.2： MPU6050：一阶互补滤波、二阶互补滤波和卡尔曼滤波获取欧拉角相关推荐

【51单片机快速入门指南】6.4：DHT11、DHT22单总线温湿度传感器
目录硬知识 DHT11 DHT22 通信协议读取步骤数据解读 DHT11 DHT22 示例程序 DHT11_22.c DHT11_22.h 测试程序 main.c 实验现象 DHT11 DHT2 ...
【51单片机快速入门指南】4.6：I2C 与 PCF8563实时时钟日历芯片
目录硬知识概述特性功能描述报警功能模式定时器模式 CLKOUT输出复位低电压检测器和时钟监视器低电压检测器和时钟监视器寄存器结构寄存器概述 BCD编码格式寄存器概述 Control ...
【51单片机快速入门指南】6.3：DS18B20 单总线数字温度计的多路读取
目录硬知识 DS18B20介绍时序初始化时序写时序读时序命令 ROM 操作命令 ROM 搜索举例存贮器操作命令示例程序 DS18B20.c DS18B20.h 测试程序定时器中断服务 ...
【51单片机快速入门指南】6.1：LCD1602的八线、四线控制及自定义符号，完美兼容Proteus仿真
目录硬知识显示特性接口定义操作时序写操作时序读操作时序寄存器忙标志位BF 地址计数器(AC) 显示数据寄存器(DDRAM) CGROM CGRAM 指令清屏指令光标归位指令进入模 ...
【51单片机快速入门指南】5.3：SPI控制晶联讯JLX12864G_08602 LCD屏幕
目录示例程序 JLX12864G_08602.c JLX12864G_08602.h JLX12864G_08602_Font.c JLX12864G_08602_Font.h 测试程序 main. ...
【51单片机快速入门指南】5.1：SPI与DS1302时钟芯片
目录硬知识 DS1302 简介 DS1302 使用控制寄存器日历/时钟寄存器 DS1302 的读写时序电路设计示例程序 DS1302.c DS1302.h 测试程序 main.c 实验现象 ...
【51单片机快速入门指南】4.5：I2C 与 TCA6416实现双向 IO 扩展
目录硬知识 IO 扩展芯片 TCA6416A TAC6416A 的寄存器 IO 输入寄存器 IO 输出寄存器 IO 反相寄存器 IO 方向寄存器 TCA6416A 的操作 TCA6416A 写数据 ...
【51单片机快速入门指南】4.4.3：Madgwick AHRS 九轴姿态融合获取四元数、欧拉角
目录传感器的方向源码 Madgwick_9.c Madgwick_9.h 使用方法测试 main.c 效果 STC15F2K60S2 22.1184MHz Keil uVision V5.29. ...
【51单片机快速入门指南】4.4.2：Mahony AHRS 九轴姿态融合获取四元数、欧拉角
目录传感器的方向源码 Mahony_9.c Mahony_9.h 使用方法测试 main.c 效果 STC15F2K60S2 22.1184MHz Keil uVision V5.29.0.0 ...
【51单片机快速入门指南】4.4.1：python串口接收磁力计数据并进行最小二乘法椭球拟合
目录硬知识 Python代码使用方法串口收集数据椭球拟合验证 STC15F2K60S2 16.384MHz Keil uVision V5.29.0.0 PK51 Prof.Develope ...

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

目录

源码