【STM32 HAL库】MPU6050姿态解算 卡尔曼滤波

前言

本篇文章基于卡尔曼滤波的原理详解与公式推导，来详细的解释下如何使用卡尔曼滤波来解算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   :  [e-mail]*//** |---------------------------------------------------------------------------------* | 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) = 200HzData = 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 -> +- 2gData = 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 °/sData = 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();  // 更新计时器// 计算滚转角 rolldouble 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;}// 计算俯仰角 pitchdouble 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{// 卡尔曼滤波器更新俯仰角度 YDataStruct->KalmanAngleY = Kalman_getAngle(&KalmanY, pitch, DataStruct->Gy, dt);}// 如果俯仰角绝对值超过 90 度，则反转 X 轴的陀螺仪角速度，防止符号错误if (fabs(DataStruct->KalmanAngleY) > 90)DataStruct->Gx = -DataStruct->Gx;// 卡尔曼滤波器更新滚转角度 XDataStruct->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;// 卡尔曼增益 Kdouble 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. 更新协方差矩阵 Pdouble 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
可以打印输出