前面提到MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角（加速度不能得到 Yaw 角），就是说有两组 Pitch、Roll 角。 MPU6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，且通过算 tan() 可以得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以，不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角，需要融合使用，这就是为什么要采用滤波的方法的原因了！
2、程序设计这里我们首先定义两个卡尔曼参数的结构体，如下所示 编写卡尔曼融合函数（这里可以直接从那篇知乎文章借鉴） 将角速度，加速度数据处理放到卡尔曼处理函数中 将初始化代码，加入并在程序中验证 将程序下载到开发板，并进入调试查看参数
3、源码kalman.c
/* * kalman.c * * Created on: Mar 21, 2023 * Author: LX */#include #include "kalman.h"#define RAD_TO_DEG 57.295779513082320876798154814105extern IMU_Parameter IMU_Data;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,};void MPU6050_Read_All(IMU_Parameter *IMU_Data){ double dt = (double)(HAL_GetTick() - timer) / 1000; timer = HAL_GetTick(); double roll; double roll_sqrt = sqrt(IMU_Data->Accel_X * IMU_Data->Accel_X + IMU_Data->Accel_Z * IMU_Data->Accel_Z); if (roll_sqrt != 0.0) { roll = atan(IMU_Data->Accel_Y / roll_sqrt) * RAD_TO_DEG; } else { roll = 0.0; } double pitch = atan2(-IMU_Data->Accel_X, IMU_Data->Accel_Z) * RAD_TO_DEG; if ((pitch KalmanAngleY > 90) || (pitch > 90 && IMU_Data->KalmanAngleY KalmanAngleY = pitch; } else { IMU_Data->KalmanAngleY = Kalman_getAngle(&KalmanY, pitch, IMU_Data->Gyro_Y, dt); } if (fabs(IMU_Data->KalmanAngleY) > 90) IMU_Data->Gyro_X = -IMU_Data->Gyro_X; IMU_Data->KalmanAngleX = Kalman_getAngle(&KalmanX, roll, IMU_Data->Gyro_X, dt);}double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt){ double rate = newRate - Kalman->bias; Kalman->angle += dt * rate; 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; double S = Kalman->P[0][0] + Kalman->R_measure; double K[2]; K[0] = Kalman->P[0][0] / S; K[1] = Kalman->P[1][0] / S; double y = newAngle - Kalman->angle; Kalman->angle += K[0] * y; Kalman->bias += K[1] * y; 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; return Kalman->angle;};kalman.h
/* * kalman.h * * Created on: Mar 21, 2023 * Author: LX */#ifndef KALMAN_H_#define KALMAN_H_#include "main.h"#include #include "i2c.h"#include "mpu6050.h"typedef struct { double Q_angle; double Q_bias; double R_measure; double angle; double bias; double P[2][2];} Kalman_t;void MPU6050_Read_All(IMU_Parameter *IMU_Data);double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt);#endif /* KALMAN_H_ */