MPU6050使用卡尔曼滤波来获取数据

关于滤波的方法来获取数据我们在第一篇文章其实就讲到了，但是没有讲怎么去使用他，因此这里就挑选一个用起来比较好的卡尔曼滤波来说明下，关于其他的比较好的滤波方式，可以见：https://zhuanlan.zhihu.com/p/228805569

1、使用滤波来获取姿态角

前面提到MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角（加速度不能得到 Yaw 角），就是说有两组 Pitch、Roll 角。
MPU6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，且通过算 tan() 可以得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以，不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角，需要融合使用，这就是为什么要采用滤波的方法的原因了！

2、程序设计

这里我们首先定义两个卡尔曼参数的结构体，如下所示

编写卡尔曼融合函数（这里可以直接从那篇知乎文章借鉴）

将角速度，加速度数据处理放到卡尔曼处理函数中

将初始化代码，加入并在程序中验证

将程序下载到开发板，并进入调试查看参数

3、源码

kalman.c

/*
 * kalman.c
 *
 *  Created on: Mar 21, 2022
 *      Author: LX
 */

#include 
#include "kalman.h"

#define RAD_TO_DEG 57.295779513082320876798154814105

extern 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 < -90 && IMU_Data->KalmanAngleY > 90) || (pitch > 90 && IMU_Data->KalmanAngleY < -90))
    {
        KalmanY.angle = pitch;
        IMU_Data->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, 2022
 *      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_ */