MPU6050在STM32中使用卡尔曼滤波计算姿态角_mpu6050卡尔曼滤波姿态解算stm32

目录

一、卡尔曼滤波的五大步骤

二、MPU6050计算姿态角

  1、加速度计（accelerometer）计算姿态角        

   2、陀螺仪（gyroscope）计算姿态角

三、使用卡尔曼滤波计算MPU6050姿态角

一、卡尔曼滤波的五大步骤

1.         先验估计：
2.         先验误差协方差：
3.         卡尔曼增益计算： 
4.         计算后验估计： 
5.         更新误差协方差矩阵 ：  

二、MPU6050计算姿态角

  1、加速度计（accelerometer）计算姿态角        

        翻滚角计算：

        俯仰角计算：

其中   (ax为读取到的加速度X轴的值，选择加速度计量程为 

g),AY的计算同理。

   2、陀螺仪（gyroscope）计算姿态角

        翻滚角计算：

        俯仰角计算：

        偏航角计算：

其中： （gx为读取到的陀螺仪的X轴的值，

陀螺仪量程选择  °/s），GY、GZ的计算同理

三、使用卡尔曼滤波计算MPU6050姿态角

将陀螺仪计算翻滚角和俯仰角的公式

写成：

就可以得出先验估计矩阵，其中

在将加速度计得到的翻滚角和俯仰角作为测量值z

设置测量误差协方差，估算误差协方差

在STM32中实现的代码如下：

static float A[2][2] = {{1,0},{0,1}};static float H[2][2] = {{1,0},{0,1}};static float Q[2][2] = {{0.0025,0},{0,0.0025}};static float R[2][2] = {{0.3,0},{0,0.3}};static float Z[2]; //加速度计的测量值static float X_hat_p[2]; //X_hat_p = {roll_g,pitch_g}static float X_hat[2] = {0,0};static float P_e[2][2] = {{1,0},{0,1}};static float P_e_p[2][2];static float K[2][2]; static float B[2][2] = {{0.005,0},{0,0.005}}; //采样时间设为∆t = 5msstatic float U[2] ;float Yaw_g;static float AX,AY,AZ,GX,GY,GZ,Temp;void Kal_Init(void){ //该函数获得MPU6050六个轴的传感器值并进行了归一化处理，换算成标准单位 MPU6050_GetData(&AX,&AY,&AZ,&GX,&GY,&GZ,&Temp); U[0] = GX; U[1] = GY; Yaw_g = Yaw_g + GZ * 0.005; Z[0] = atan2(AY,AZ)/ 3.1415927f * 180.0f; Z[1] = atan2(AX,AZ)/ 3.1415927f * 180.0f; //1.计算先验估计 X_hat_p[0] = X_hat[0] + U[0] * B[0][0]; X_hat_p[1] = X_hat[1] + U[1] * B[1][1]; //2.计算先验误差协方差 P_e_p[0][0] = P_e[0][0] + Q[0][0]; P_e_p[0][1] = 0; P_e_p[1][0] = 0; P_e_p[1][1] = P_e[1][1] + Q[1][1]; //3.计算卡尔曼增益 K[0][0] = P_e_p[0][0] /(P_e_p[0][0] + R[0][0]); K[0][1] = 0; K[1][0] = 0; K[1][1] = P_e_p[1][1] / (P_e_p[1][1] + R[1][1]); //4.计算后验估计 X_hat[0] = X_hat_p[0] + K[0][0] * (Z[0] - X_hat_p[0]); X_hat[1] = X_hat_p[1] + K[1][1] * (Z[1] - X_hat_p[1]); //5.更新误差协方差 P_e[0][0] = (1 - K[0][0]) * P_e_p[0][0]; P_e[1][1] = (1 - K[1][1]) * P_e_p[1][1]; Yaw_g = Yaw_g - 0.002; Delay_ms(5); //设置采样时间为5ms }