【STM32 HAL库】MPU6050姿态解算 卡尔曼滤波
- 前言
- MPU6050寄存器
- 代码详解
- mpu6050.c
- mpu6050.h
- 使用说明
前言
本篇文章基于卡尔曼滤波的原理详解与公式推导，来详细的解释下如何使用卡尔曼滤波来解算MPU6050的姿态
参考资料：Github_mpu6050
MPU6050寄存器
我们简单介绍下MPU6050驱动里设计到的寄存器，详情见MPU-60X0寄存器手册
Who Am I 寄存器
为数不多的默认值不为0的寄存器，其默认值为0x68，也即MPU6050的设备地址，通过读取该寄存器的值来判断识别到的设备是否是MPU6050
Power Management 1 寄存器
通过该寄存器配置电源模式、时钟源选择、设备复位和睡眠模式等功能。
Sample Rate Divider 寄存器
采样速率分频器，通过该寄存器设置分频系数，配置 MPU6050 数据输出速率。
CONFIGURATION 寄存器
有GYROSCOPE CONFIGURATION 和 ACCELEROMETER CONFIGURATION 寄存器，用来配置陀螺仪、加速度计参数
详情见MPU-60X0寄存器手册
MEASUREMENT 寄存器
用来存放加速度计、温度计、陀螺仪测量到的原始数据
详情见MPU-60X0寄存器手册
代码详解
mpu6050.c
/*
* mpu6050.c
*
* Created on: Nov 13, 2019
* Author: Bulanov Konstantin
*
* Contact information
* -------------------
*
* e-mail : leech001@gmail.com
*/
/*
* |---------------------------------------------------------------------------------
* | Copyright (C) Bulanov Konstantin,2021
* |
* | This program is free software: you can redistribute it and/or modify
* | it under the terms of the GNU General Public License as published by
* | the Free Software Foundation, either version 3 of the License, or
* | any later version.
* |
* | This program is distributed in the hope that it will be useful,
* | but WITHOUT ANY WARRANTY; without even the implied warranty of
* | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* | GNU General Public License for more details.
* |
* | You should have received a copy of the GNU General Public License
* | along with this program. If not, see <http://www.gnu.org/licenses/>.
* |
* | Kalman filter algorithm used from https://github.com/TKJElectronics/KalmanFilter
* |---------------------------------------------------------------------------------
*/
#include <math.h>
#include "mpu6050.h"
#define RAD_TO_DEG 57.295779513082320876798154814105 //用以弧度转角度，角度 = 弧度 × (180/π)，其中 180/π 的值约为 57.2958
#define WHO_AM_I_REG 0x75 // MPU6050 的 WHO_AM_I 寄存器地址。通过读取该寄存器的值，如果返回 0x68，则表示该 I2C 设备是 MPU6050 传感器。
#define PWR_MGMT_1_REG 0x6B // MPU6050 的 Power Management 1 寄存器地址 (电源管理 1 寄存器)，用于配置电源模式、时钟源选择、设备复位和睡眠模式等功能。
#define SMPLRT_DIV_REG 0x19 // MPU6050 的 Sample Rate Divider 寄存器地址 (采样速率分频器)，通过该寄存器设置分频系数，配置 MPU6050 数据输出速率。
#define ACCEL_CONFIG_REG 0x1C // MPU6050 的 ACCEL_CONFIG 寄存器地址 (加速度计配置寄存器)，用于设置加速度计的量程范围（例如 ±2g、±4g 等）。
#define ACCEL_XOUT_H_REG 0x3B // MPU6050 的加速度计 X 轴高位数据寄存器地址，用于读取 X 轴加速度高字节数据。
#define TEMP_OUT_H_REG 0x41 // MPU6050 的温度传感器高位数据寄存器地址，用于读取温度的高字节数据。
#define GYRO_CONFIG_REG 0x1B // MPU6050 的 GYRO_CONFIG 寄存器地址 (陀螺仪配置寄存器)，用于设置陀螺仪的量程范围（例如 ±250°/s、±500°/s 等）。
#define GYRO_XOUT_H_REG 0x43 // MPU6050 的陀螺仪 X 轴高位数据寄存器地址，用于读取 X 轴陀螺仪角速度的高字节数据。
// Setup MPU6050
#define MPU6050_ADDR 0xD0 // MPU6050 的补位后的 8 位地址，MPU6050的 7 位地址为 110 100x (0x68 或 0x69)，最低位 x 取决于 AD0 引脚状态。I2C 通信时，左移一位并在最低位加 0（写）或 1（读），形成 8 位地址。以写为例，MPU6050 地址为 1101 0000 (0xD0) 表示写操作
const uint16_t i2c_timeout = 100; // I2C 通信超时时间，单位为毫秒。
const double Accel_Z_corrector = 14418.0;
uint32_t timer;
Kalman_t KalmanX = {
.Q_angle = 0.001f,
.Q_bias = 0.003f,
.R_measure = 0.03f};
Kalman_t KalmanY = {
.Q_angle = 0.001f,
.Q_bias = 0.003f,
.R_measure = 0.03f,
};
uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx)
{
uint8_t check; // 存储从 WHO_AM_I 寄存器读取到的设备 ID，实际上是该寄存器的值0x68（该寄存器默认值为 0x68 (104)，也即 MPU6050 的地址
uint8_t Data;
HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, WHO_AM_I_REG, 1, &check, 1, i2c_timeout); // 通过 I2Cx 访问 MPU6050 的 WHO_AM_I 寄存器，读取 1 字节数据到 check，超时时间为 i2c_timeout。
if (check == 104) // 检验 check 的值 是否为 0x68
{
// 设置 Power Management 1 寄存器 (电源管理1寄存器)，将其第7位 置1 (1000 0000) 进行设备复位，防止数据残留
Data = 0x80;
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, PWR_MGMT_1_REG, 1, &Data, 1, i2c_timeout);
// 设置 Power Management 1 寄存器 (电源管理1寄存器)，将其所有位置0 (0000 0000) ，进行唤醒
Data = 0;
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, PWR_MGMT_1_REG, 1, &Data, 1, i2c_timeout);
// 设置 Sample Rate Divider 寄存器 (采样速率分频器)，采样速率 = 1kHz / ( 0X04 + 1) = 200Hz
Data = 0x04;
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, SMPLRT_DIV_REG, 1, &Data, 1, i2c_timeout);
//设置 ACCEL_CONFIG 寄存器 (加速度计配置 Accelerometer Configuration )
// 关闭X Y Z轴的自检功能，配置量程为(-2g,+2g) XA_ST=0,YA_ST=0,ZA_ST=0, FS_SEL=0 -> +- 2g
Data = 0x00;
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, ACCEL_CONFIG_REG, 1, &Data, 1, i2c_timeout);
//设置 GYRO_CONFIG 寄存器 (陀螺仪配置 Gyroscopic Configuration )
// 关闭X Y Z轴的自检功能，并配置量程为 (-250 °/s,+250 °/s) XG_ST=0, YG_ST=0, ZG_ST=0 , FS_SEL=0 -> ±250 °/s
Data = 0x00;
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, GYRO_CONFIG_REG, 1, &Data, 1, i2c_timeout);
return 0; //返回0，表示 MPU6050 正常
}
return 1; //返回1，表示 MPU6050 异常
}
void MPU6050_Read_Accel(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{
uint8_t Rec_Data[6]; //存储读取到的 6 字节加速度计原始数据
// 通过 I2Cx 访问 MPU6050 的 Accelerometer Measurements 寄存器组中的 ACCEL_XOUT_H_REG 寄存器(加速度计X轴加速度高字节 寄存器)，读取 6 字节数据( X轴加速度的高字节 ---> Z轴加速度的低字节 )到 Rec_Data ，超时时间为 i2c_timeout。
HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 6, i2c_timeout);
//将读取到的存储在 Rec_Data[6] 中的原始数据 分离转换为 X、Y、Z轴加速度的 16 位有符号整数
DataStruct->Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
DataStruct->Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
DataStruct->Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
/*** 将原始值转换为以 'g' 为单位的加速度值。
根据 FS_SEL 配置来进行换算，这里 FS_SEL = 0，对应量程为 ±2g，
因此需要将原始值除以 16384.0，来得到加速度值。
根据 Accelerometer Measurements 加速度计测量寄存器组中的 AFS_SEL 配置中的描述，可知量程对应的除数
更多细节请参考 ACCEL_CONFIG 与 Accelerometer Measurements寄存器的配置。 ****/
// 将原始值转换为 g 为单位的加速度
DataStruct->Ax = DataStruct->Accel_X_RAW / 16384.0; // X 轴加速度
DataStruct->Ay = DataStruct->Accel_Y_RAW / 16384.0; // Y 轴加速度
DataStruct->Az = DataStruct->Accel_Z_RAW / Accel_Z_corrector; //Z 轴加速度 (由于Z轴加速度计的值会受重力、传感器零飘、传感器精度与校准的影响，导致在水平放置时，Accel_Z_RAW 不为 2g 量程对应的理论值 16384，而是实际值。修正参数就是根据实际情况测出的，用来矫正零飘和其他影响的校正因子
}
void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{
uint8_t Rec_Data[6]; //存储读取到的 6 字节陀螺仪计原始数据
// 通过 I2Cx 访问 MPU6050 的 Gyroscope Measurements 寄存器组中的 GYRO_XOUT_H_REG 寄存器(陀螺仪 X 轴角速度高字节 寄存器)，读取 6 字节数据( X 轴角速度的高字节 ---> Z 轴角速度的低字节 )到 Rec_Data ，超时时间为 i2c_timeout。
HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, GYRO_XOUT_H_REG, 1, Rec_Data, 6, i2c_timeout);
//将读取到的存储在 Rec_Data[6] 中的原始数据 分离转换为 X、Y、Z轴角速度的 16 位有符号整数
DataStruct->Gyro_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
DataStruct->Gyro_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
DataStruct->Gyro_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
/*** 将原始值转换为度每秒（dps，degrees per second）。
根据 FS_SEL 的配置选择适当的比例系数。这里假设 FS_SEL = 0，对应量程为 ±250 °/s，
因此需要将原始值除以 131.0 来得到角速度（dps）。
根据 Gyroscope Measurements 角速度计测量寄存器组中的 FS_SEL 配置中的描述，可知量程对应的除数
更多细节请参考 GYRO_CONFIG 与 Gyroscope Measurements寄存器的配置。 ****/
// 将原始值转换为 dps 为单位的加速度
DataStruct->Gx = DataStruct->Gyro_X_RAW / 131.0; // X 轴角速度
DataStruct->Gy = DataStruct->Gyro_Y_RAW / 131.0; // Y 轴角速度
DataStruct->Gz = DataStruct->Gyro_Z_RAW / 131.0; // Z 轴角速度
}
void MPU6050_Read_Temp(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{
uint8_t Rec_Data[2]; // 用于存储从温度寄存器读取的 2 字节数据
int16_t temp; // 用于存储拼接后的 16 位原始温度数据
// 从 Temperature Measurement 温度测量寄存器组 中的 TEMP_OUT_H 寄存器开始，读取 2 字节数据（高字节和低字节）
HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, TEMP_OUT_H_REG, 1, Rec_Data, 2, i2c_timeout);
// 将读取到的高字节和低字节拼接为 16 位的有符号整数
temp = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
// 将原始温度值转换为摄氏温度 (详情见 Temperature Measurement
// 转换公式：温度值(°C) = (原始值 / 340.0) + 36.53
DataStruct->Temperature = (float)((int16_t)temp / (float)340.0 + (float)36.53);
}
/**
* @brief 读取 MPU6050 所有传感器数据，并使用卡尔曼滤波器计算角度
*
* 该函数通过 I2C 接口读取 MPU6050 传感器的加速度计、陀螺仪和温度的数据，
* 并使用卡尔曼滤波器对预测角度与测量角度(加速度计和陀螺仪数据)进行融合，以计算俯仰角（pitch）和滚转角（roll）。
*
* @param I2Cx I2C 句柄，用于通过 I2C 接口与 MPU6050 通信
* @param DataStruct 指向 MPU6050_t 结构体的指针，存储读取到的原始传感器数据以及处理后的数据（如加速度、角速度、角度等）
*/
void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{
uint8_t Rec_Data[14]; // 用于存储从 MPU6050 读取的 14 字节数据
int16_t temp; // 临时变量，用于存储原始温度数据
//1.通过 I2C 接口从 MPU6050 读取 14 字节加速度、温度、角速度数据
HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 14, i2c_timeout);
//2.解析数据
// 解析加速度计数据
DataStruct->Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
DataStruct->Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
DataStruct->Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
// 解析温度数据
temp = (int16_t)(Rec_Data[6] << 8 | Rec_Data[7]);
// 解析陀螺仪数据
DataStruct->Gyro_X_RAW = (int16_t)(Rec_Data[8] << 8 | Rec_Data[9]);
DataStruct->Gyro_Y_RAW = (int16_t)(Rec_Data[10] << 8 | Rec_Data[11]);
DataStruct->Gyro_Z_RAW = (int16_t)(Rec_Data[12] << 8 | Rec_Data[13]);
//3.转换解析后的数据： 原始数据 --> 实际意义数据
// 将原始加速度计数据 ---> 以 "g" 为单位的加速度值
DataStruct->Ax = DataStruct->Accel_X_RAW / 16384.0;
DataStruct->Ay = DataStruct->Accel_Y_RAW / 16384.0;
DataStruct->Az = DataStruct->Accel_Z_RAW / Accel_Z_corrector;
// 将原始温度数据 ---> 摄氏温度
DataStruct->Temperature = (float)((int16_t)temp / (float)340.0 + (float)36.53);
// 将原始陀螺仪数据 ---> 角速度(以 "度每秒 (dps)" 为单位
DataStruct->Gx = DataStruct->Gyro_X_RAW / 131.0;
DataStruct->Gy = DataStruct->Gyro_Y_RAW / 131.0;
DataStruct->Gz = DataStruct->Gyro_Z_RAW / 131.0;
//4.卡尔曼滤波
// 计算时间增量 dt，单位为秒
double dt = (double)(HAL_GetTick() - timer) / 1000; // 获取时间差（毫秒），转换为秒
timer = HAL_GetTick(); // 更新计时器
// 计算滚转角 roll
double roll; // 用于存储计算得到的滚转角（X 轴）
double roll_sqrt = sqrt(
DataStruct->Accel_X_RAW * DataStruct->Accel_X_RAW + DataStruct->Accel_Z_RAW * DataStruct->Accel_Z_RAW);
if (roll_sqrt != 0.0)
{
roll = atan(DataStruct->Accel_Y_RAW / roll_sqrt) * RAD_TO_DEG; // 先计算出弧度制 roll ，再弧度转换为角度值
}
else
{
roll = 0.0;
}
// 计算俯仰角 pitch
double pitch = atan2(-DataStruct->Accel_X_RAW, DataStruct->Accel_Z_RAW) * RAD_TO_DEG;
// 如果俯仰角度变化过快(超过90度)，防止角度跳变
if ((pitch < -90 && DataStruct->KalmanAngleY > 90) || (pitch > 90 && DataStruct->KalmanAngleY < -90))
{
KalmanY.angle = pitch;
DataStruct->KalmanAngleY = pitch;
}
else
{
// 卡尔曼滤波器更新俯仰角度 Y
DataStruct->KalmanAngleY = Kalman_getAngle(&KalmanY, pitch, DataStruct->Gy, dt);
}
// 如果俯仰角绝对值超过 90 度，则反转 X 轴的陀螺仪角速度，防止符号错误
if (fabs(DataStruct->KalmanAngleY) > 90)
DataStruct->Gx = -DataStruct->Gx;
// 卡尔曼滤波器更新滚转角度 X
DataStruct->KalmanAngleX = Kalman_getAngle(&KalmanX, roll, DataStruct->Gx, dt);
}
/**
* @brief 使用卡尔曼滤波，融合角度预测值与角度测量值，得到最优角度值
*
* 卡尔曼滤波通过结合系统的预测值和测量值，来修正角度和偏置的估计，减少噪声对结果的影响。
*
* @param Kalman 卡尔曼滤波器结构体指针，包含预测角度、预测偏置、角度协方差、偏置协方差、噪声协方差及协方差矩阵
* @param newAngle 角度测量值（读取加速度计的三轴加速度分量，再计算反正切得到角度测量值）
* @param newRate 角速度“实际“值（他只是可以看作角速度实际值，实际上是角速度测量值。只不过因陀螺仪精度问题，而有过程噪声。可以理解为匀变速直线运动的状态方程中必须有 速度v 的参与，这个 v 实际上也是测量值）
* @param dt 时间间隔，两个传感器数据采样之间的时间差（秒）
* @return double 滤波后的最优角度值
*/
double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt)
{
/*---------------------预测阶段--------------------------*/
// 1. 预测角度
// 角速度 = 陀螺仪角速度 - 陀螺仪偏置值 (得到无偏角速度)
double rate = newRate - Kalman->bias;
// 预测角度 = 前一时刻角速 + 时间间隔*角速度
Kalman->angle += dt * rate;
// 2. 预测协方差矩阵
Kalman->P[0][0] += dt * (dt * Kalman->P[1][1] - Kalman->P[0][1] - Kalman->P[1][0] + Kalman->Q_angle); // 预测角度协方差
Kalman->P[0][1] -= dt * Kalman->P[1][1]; // 预测角度和偏置的协方差
Kalman->P[1][0] -= dt * Kalman->P[1][1]; // 预测偏置和角度的协方差
Kalman->P[1][1] += Kalman->Q_bias * dt; // 预测偏置协方差
/*---------------------更新阶段--------------------------*/
// 3. 更新卡尔曼增益
// 总误差协方差 = 预测协方差 + 测量噪声协方差
double S = Kalman->P[0][0] + Kalman->R_measure;
// 卡尔曼增益 K
double K[2];
K[0] = Kalman->P[0][0] / S; // 角度的卡尔曼增益
K[1] = Kalman->P[1][0] / S; // 偏置的卡尔曼增益
// 4. 更新角度和偏置
// 测量残差 = 测量值 - 预测值
double y = newAngle - Kalman->angle;
// 根据卡尔曼增益，更新角度和偏置的估计值，修正预测阶段的误差
Kalman->angle += K[0] * y; // 更新角度估计。
Kalman->bias += K[1] * y; // 更新偏置估计
// 5. 更新协方差矩阵 P
double P00_temp = Kalman->P[0][0];
double P01_temp = Kalman->P[0][1];
Kalman->P[0][0] -= K[0] * P00_temp; // 更新角度协方差
Kalman->P[0][1] -= K[0] * P01_temp; // 更新角度和偏置的协方差
Kalman->P[1][0] -= K[1] * P00_temp; // 更新偏置和角度的协方差
Kalman->P[1][1] -= K[1] * P01_temp; // 更新偏置协方差
// 6. 返回滤波后的最优角度值
return Kalman->angle;
};
mpu6050.h
/*
* mpu6050.h
*
* Created on: Nov 13, 2019
* Author: Bulanov Konstantin
* 本头文件定义了用于操作 MPU6050 传感器的结构和函数，包括加速度计、陀螺仪、温度传感器的读取函数，
* 以及用于角度计算的卡尔曼滤波算法。
*/
#ifndef INC_GY521_H_
#define INC_GY521_H_
#endif /* INC_GY521_H_ */
#include <stdint.h>
#include "i2c.h"
/*
* MPU6050 数据结构体
* 该结构体保存从 MPU6050 传感器读取的原始加速度和原始陀螺仪数据
* 以及经过处理后的加速度、角速度和温度数据
*/
typedef struct
{
int16_t Accel_X_RAW; // X 轴加速度原始数据
int16_t Accel_Y_RAW; // Y 轴加速度原始数据
int16_t Accel_Z_RAW; // Z 轴加速度原始数据
double Ax; // X 轴加速度值（g）
double Ay; // Y 轴加速度值（g）
double Az; // Z 轴加速度值（g）
int16_t Gyro_X_RAW; // X 轴陀螺仪原始数据
int16_t Gyro_Y_RAW; // Y 轴陀螺仪原始数据
int16_t Gyro_Z_RAW; // Z 轴陀螺仪原始数据
double Gx; // X 轴角速度值（°/s）
double Gy; // Y 轴角速度值（°/s）
double Gz; // Z 轴角速度值（°/s）
float Temperature; // 传感器的温度（°C）
double KalmanAngleX; // X 轴的卡尔曼滤波计算角度
double KalmanAngleY; // Y 轴的卡尔曼滤波计算角度
} MPU6050_t;
/*
* 卡尔曼滤波器结构体
* 用于根据 MPU6050 数据平滑地估算出角度，滤除噪声并提供更稳定的输出
*/
typedef struct
{
double Q_angle; // 角度过程噪声协方差
double Q_bias; // 偏差过程噪声协方差
double R_measure; // 测量噪声协方差
double angle; // 当前估计角度
double bias; // 当前估计偏差
double P[2][2]; // 误差协方差矩阵
} Kalman_t;
// 初始化 MPU6050 传感器，配置 MPU6050 参数，返回 0 表示成功，1 表示失败
uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx);
// 读取 MPU6050 加速度计数据，更新到 mpu6050 结构体中
void MPU6050_Read_Accel(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);
// 读取 MPU6050 陀螺仪数据，更新到 mpu6050 结构体中
void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);
// 读取 MPU6050 温度数据，更新到 mpu6050 结构体中
void MPU6050_Read_Temp(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);
// 读取 MPU6050 的加速度计、陀螺仪和温度数据，并更新到 mpu6050 结构体中
void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);
// 使用卡尔曼滤波器计算角度，根据新测得的角度和角速度更新滤波器，返回平滑的角度值
double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt);
使用说明
STEP1：复制mpu6050的.c .h到你的工程文件夹中，并add文件与编译路径
STEP2：Includes
#include "mpu6050.h"
STEP3：声明私有变量PV
/ * USER CODE BEGIN PV * /
MPU6050_t MPU6050;
/ * USER CODE END PV * /
STEP4：初始化MPU6050
void setup(void){
while (MPU6050_Init(&hi2c1) == 1);
}
STEP5：调用函数读取并解析数据
void loop(void){
MPU6050_Read_All(&hi2c1, &MPU6050);
HAL_Delay (100);
}
经过卡尔曼滤波后得到的数据(最优估计)为MPU6050.KalmanAngleX和MPU6050.KalmanAngleY
可以打印输出
