基于英飞凌TC264单片机——ICM20602/MPU6050从原始数据到欧拉角代码（卡尔曼滤波）

最新推荐文章于 2023-12-03 19:54:54 发布

AIlways

最新推荐文章于 2023-12-03 19:54:54 发布

阅读量1.4w

点赞数 46

分类专栏：学习笔记文章标签：经验分享

本文链接：https://blog.csdn.net/qq_43745620/article/details/107834899

版权

学习笔记专栏收录该内容

15 篇文章 7 订阅

订阅专栏

卡尔曼滤波解算欧拉角

初始化
主要函数

初始化

CPU1初始化代码

#include "..\Driver\include.h"//各个模块的头文件
extern IfxCpu_mutexLock mutexCpu0InitIsOk;   /** CPU0 初始化完成标志位  */
int core1_main (void)
{
    IfxCpu_enableInterrupts();
    /*
     * 关闭看门狗
     */
    IfxScuWdt_disableCpuWatchdog (IfxScuWdt_getCpuWatchdogPassword ());

    //等待CPU0 初始化完成
    while(!IfxCpu_acquireMutex(&mutexCpu0InitIsOk));

    /* 用户代码 */
   	//OLED初始化  
	OLED_Init();	
	//MPU6050初始化
    MPU6050Init();
    //设置定时中断 5000us
    CCU6_InitConfig(CCU61, CCU6_Channel1, 5000);
#pragma warning 557         // 屏蔽警告
    while(1)//主循环
    {
		;
    }
#pragma warning default     // 打开警告
}

MPU6050初始化

void MPU6050Init(void)
{
    //陀螺仪IIC初始化
    IIC_Init();
	if(MPU6050_Init())
    {
        OLED_P6x8Str(0,0,"6050 Test Fail \r\n");
#pragma warning 557         // 屏蔽警告
		while (1);
#pragma warning default     // 打开警告
    }
}

主要函数

中断服务函数

void CCU61_CH1_IRQHandler(void)
{
	/* 开启CPU中断  否则中断不可嵌套 */
	IfxCpu_enableInterrupts();

	//清除中断标志
	IfxCcu6_clearInterruptStatusFlag(&MODULE_CCU61, IfxCcu6_InterruptSource_t13PeriodMatch);

	/* 用户代码 */
	c++;
	if(c == 2)
	{
		MPU6050();
		c = 0;
		sprintf(txt,"Angle_X:%06f",Angle_X_Final);
		OLED_P6x8Str(0,0,txt);         //字符串
		sprintf(txt,"Angle_Y:%06f",Angle_Y_Final);
		OLED_P6x8Str(0,1,txt);         //字符串
		OLED_CLS();
	}
}

MPU6050子函数代码

void MPU6050(void)
{
    MPU_Get_Raw_data(&Accel_x,&Accel_y,&Accel_z,&Gyro_x,&Gyro_y,&Gyro_z);	//得到加速度传感器数据
        ax = (9.8*Accel_x)/8192;
        ay = (9.8*Accel_y)/8192;
        az = (9.8*Accel_z)/8192;
        Gyro_x = (Gyro_x)/16.4;
        Gyro_y = (Gyro_y)/16.4;
        Gyro_z = (Gyro_z)/16.4;

    	//用加速度计算三个轴和水平面坐标系之间的夹角
    	Angle_x_temp=(atan(ay/az))*180/3.14;
    	Angle_y_temp=(atan(ax/az))*180/3.14;

    	Kalman_Filter_X(Angle_x_temp,Gyro_x);  //卡尔曼滤波计算X倾角
    	Kalman_Filter_Y(Angle_y_temp,Gyro_y);  //卡尔曼滤波计算Y倾角
}

卡尔曼滤波（这段代码网上不知道谁发的，直接拿来用了，感谢作者）

//卡尔曼参数
float Q_angle = 0.001;
float Q_gyro  = 0.003;
float R_angle = 0.5;
float dt      = 0.01;//dt为kalman滤波器采样时间
char  C_0     = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };

void Kalman_Filter_X(float Accel,float Gyro) //卡尔曼函数
{
	Angle_X_Final += (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_X_Final;	//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_X_Final += K_0 * Angle_err;	 //后验估计
	Q_bias        += K_1 * Angle_err;	 //后验估计
	Gyro_x         = Gyro - Q_bias;	 //输出值（后验估计）的微分 = 角速度
}

void Kalman_Filter_Y(float Accel,float Gyro) //卡尔曼函数
{
	Angle_Y_Final += (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_Y_Final;	//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_Y_Final	+= K_0 * Angle_err;	 //后验估计
	Q_bias	+= K_1 * Angle_err;	 //后验估计
	Gyro_y   = Gyro - Q_bias;	 //输出值（后验估计）的微分 = 角速度
}

代码主要求解出pitch和roll，yaw没有求解
ICM20602/MPU6050实测有效，上张ICM的图，角度挺准，速度也很快，动态测算挺准的，视频不知道咋传，将就看一下图片吧
在这里插入图片描述
不过解算过程耗费时间，如果想要节省单片机的计算量，也可以选择直接从MPU6050的DMP中读出四元数然后解算出欧拉角。使用英飞凌TC264的可以参考文章，效果很不错。互补滤波求欧拉角也很实用。
基于英飞凌TC264D的MPU6050调用DMP的移值.
想要了解卡尔曼滤波数学原理的也可以跳转KalmanFilter.Net
或者跳转个人推荐视频链接，这个up主很厉害
1.【卡尔曼滤波器】1_递归算法_Recursive Processing.
2.【卡尔曼滤波器】2_数学基础_数据融合_协方差矩阵_状态空间方程_观测器问题.
3.【卡尔曼滤波器】3_卡尔曼增益超详细数学推导～全网最完整.
4.【卡尔曼滤波器】4_误差协方差矩阵数学推导_卡尔曼滤波器的五个公式.
5.【卡尔曼滤波器】5_直观理解与二维实例【包含完整的EXCEL代码】.
祝大家学习愉快-_-