卡尔曼滤波在陀螺仪角度获取中的运用（最大可能解决MPU6050零漂问题）
前言：相信使用过MPU6050的人都会发现此陀螺仪（角度传感器）存在零点漂移的现象。我在网上查找了好多解决零点漂移的方法，大多数网友提供的方法都是对陀螺仪采集到的数据进行一些数据的处理，包括一些滤波算法的运用。在经过长时间的尝试后，我觉得使用卡尔曼滤波算法效果比较好，因此我将通过此文来记录卡尔曼滤波算法在解决陀螺仪零漂问题的运用。
同时为了方便读者移植，我会在文章末尾附上工程链接，需要的读者请自取。有关陀螺仪的工作原理驱动代码，读者可以参考这篇文章：STM32MPU6050角度的读取（STM32驱动MPU6050）-CSDN博客
废话不多说，直接对相关代码进行讲解：
filter.c
#include "filter.h"
/**************************************************************************
Function: Simple Kalman filter
Input : acceleration、angular velocity
Output : none
函数功能：获取x轴角度简易卡尔曼滤波
入口参数：加速度获取的角度、角速度
返回 值：x轴角速度
**************************************************************************/
float dt=0.005; //每5ms进行一次滤波
float Kalman_Filter_x(float Accel,float Gyro)
{
static float angle_dot;
static float angle;
float Q_angle=0.001; // 过程噪声的协方差
float Q_gyro=0.003; //0.003 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
float R_angle=0.5; // 测量噪声的协方差 既测量偏差
char C_0 = 1;
static float Q_bias, Angle_err;
static float PCt_0, PCt_1, E;
static float K_0, K_1, t_0, t_1;
static float Pdot[4] ={0,0,0,0};
static float PP[2][2] = { { 1, 0 },{ 0, 1 } };
angle+=(Gyro - Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]=-PP[1][1];
Pdot[2]=-PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - angle; //zk-先验估计
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
angle_dot = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
return angle;
}
/**************************************************************************
Function: First order complementary filtering
Input : acceleration、angular velocity
Output : none
函数功能：一阶互补滤波
入口参数：加速度获取的角度、角速度
返回 值：x轴角速度
**************************************************************************/
float Complementary_Filter_x(float angle_m, float gyro_m)
{
static float angle;
float K1 =0.02;
angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * dt);
return angle;
}
/**************************************************************************
Function: Simple Kalman filter
Input : acceleration、angular velocity
Output : none
函数功能：获取y轴角度简易卡尔曼滤波
入口参数：加速度获取的角度、角速度
返回 值：y轴角速度
**************************************************************************/
float Kalman_Filter_y(float Accel,float Gyro)
{
static float angle_dot;
static float angle;
float Q_angle=0.001; // 过程噪声的协方差
float Q_gyro=0.003; //0.003 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
float R_angle=0.5; // 测量噪声的协方差 既测量偏差
char C_0 = 1;
static float Q_bias, Angle_err;
static float PCt_0, PCt_1, E;
static float K_0, K_1, t_0, t_1;
static float Pdot[4] ={0,0,0,0};
static float PP[2][2] = { { 1, 0 },{ 0, 1 } };
angle+=(Gyro - Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]=-PP[1][1];
Pdot[2]=-PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - angle; //zk-先验估计
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
angle_dot = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
return angle;
}
/**************************************************************************
Function: First order complementary filtering
Input : acceleration、angular velocity
Output : none
函数功能：一阶互补滤波
入口参数：加速度获取的角度、角速度
返回 值：y轴角速度
**************************************************************************/
float Complementary_Filter_y(float angle_m, float gyro_m)
{
static float angle;
float K1 =0.02;
angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * dt);
return angle;
}
filter.h
#ifndef __FILTER_H
#define __FILTER_H
float Kalman_Filter_x(float Accel,float Gyro);
float Complementary_Filter_x(float angle_m, float gyro_m);
float Kalman_Filter_y(float Accel,float Gyro);
float Complementary_Filter_y(float angle_m, float gyro_m);
#endif
以上的卡尔曼滤波算法不仅适用于陀螺仪角度数据的处理，同时对于其他的传感器数据处理也依然适用。
这是此篇的陀螺仪卡尔曼滤波算法移植的完整工程，大家可以从以下公众号获取，公众号中回复：陀螺仪卡尔曼滤波算法
