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

目录

  • 源码
    • MPU6050_Filter.c
    • MPU6050_Filter.h
  • 使用方法
  • 测试程序
    • 一阶互补滤波
      • 效果
    • 二阶互补滤波
      • 效果
    • 卡尔曼滤波
      • 效果
  • 总结

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


参考资料:
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 
#include "./MPU6050/MPU6050_Filter.h"

#define PI 3.141592653589793

float 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 5

float 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 
#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
【51单片机快速入门指南】4.3.2: MPU6050:一阶互补滤波、二阶互补滤波和卡尔曼滤波获取欧拉角_第1张图片

二阶互补滤波

#include 
#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 
#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单片机,单片机,51单片机,卡尔曼滤波算法,姿态融合,stm32)