MPU6050六轴传感器-角度滤波（DMP+互补滤波+卡尔曼滤波）
目录
角度与角速度测量
角度滤波算法
DMP算法
互补滤波
互补滤波介绍
互补滤波算法
卡尔曼滤波
卡尔曼滤波介绍
核心代码讲解
完整代码
角度与角速度测量
角度与角速度获取
使用 MPU6050 作为姿态传感器，集成一个加速度传感器和一个陀螺仪，可以输出三轴的加速度与角速度。角速度的获取可以通过陀螺仪来直接读取，角度的获取可以有两种方法来测量：一是通过加速度计的加速度分量来计算，二是通过陀螺仪输出的角速度进行积分获得。MPU6050 的坐标系定义如图：
当传感器的正方向 Z 轴垂直指向天空时，由于此时受到地球重力的作用，此时加速度计 Z 轴的读数应为正，而且理想情况下应为𝑔。注意此时读取加速度计并不是重力加速度，而是物体自身的运动加速度，正因为自身的运动加速度与重力加速度大小相等方向相反，芯片才能保持静止。当传感器静止不动时，我们仅绕 X 轴旋转一定的角度𝜃，此时加速度方向一直与 X 轴垂直，X 轴并无加速度分量，忽略 X 轴，把加速度分解，如图 所示，可以很容易就算出传感器绕 X 轴的角度。
绕 X 轴加速度计
当仅绕 Y 轴旋转时也是同样的原理。当绕 Z 轴旋转时，因为重力加速度固定为 Z 轴方向，故在 X 与 Y 轴无加速度分量，仅仅通过加速度计无法得出绕Z轴角度，若想得到绕 Z 轴角度，只能通过角速度积分获得，但因为有偏差，一 段时间后将不再具有参考意义。物体的旋转运动就是绕三轴旋转角度的叠加，我们读取加速度计的数据，根据公式进行处理就可以获取相应的姿态角。故小车绕 X 轴与 Y 轴的角度可用以下公式算出：
在完全静止的情况下确实通过加速度计就可以获取到所需的角度，但是在实际应用中，小车因为车身摆动等情况会产生加速度，它叠加在测量信号上会无法准确地反映出车模的倾角
测量倾角与实际倾角对比
传感器安装的高度越低越能抑制因运动产生的加速度，但是还是无法切底消除这种影响。
那么通过陀螺仪的角速度进行积分得到角度呢？如果测量的角速度存在微小的误差或漂移，经过积分运算之后，形成积累误差，随着时间增加，角度信息将不再准确，这一个方法也是不太可行的。
陀螺仪积累误差
角度滤波算法
我们可以综合加速度计和陀螺仪的角度，进行滤波和平滑处理得到准确的角度。程序提供了三种得到准确的角度的算法：1.DMP 算法 2.互补滤波算法 3.卡尔曼滤波算法。
DMP算法
我们想要得到姿态角数据，即航向角、横滚角进而俯仰角数据，需要利用原始的数据进行姿态解算，而姿态解算比较复杂，难度较大，此时可以利用 MPU6050 官方自带的数字运动处理器，即 DMP
参考例程：
MPU6050模块-3轴陀螺仪和3轴加速度传感器_mpu6050 三轴陀螺仪-CSDN博客
互补滤波
互补滤波介绍
通过加速度计和陀螺仪获取的角度都有一定的缺点，加速度计获取的角度长期来看比较准确，但是波动大，可以认为其掺杂了高频噪声；陀螺仪获取的角度短时间比较准确，但有积分误差，可以认为其掺杂了低频噪声。我们可以分别让他们通过一个低通滤波器和一个高通滤波器然后叠加在一起，这就是互补滤波算法
互补滤波过程
假设这个低通滤波器其系统函数为𝐺 1 (𝑠) = 1/(𝜏𝑠 + 1)，那么高通滤波器系统函数为𝐺2 (𝑠) = 𝜏𝑠/(𝜏𝑠 + 1)，𝜏是滤波器的时间常数。可以注意到𝐺 1 (𝑠) +𝐺2 (𝑠) = 1，必须要保证滤波后增益为 1，这就是所谓“互补”。
设通过加速度计转换到的角度的拉氏变换为𝜃 𝑚 (𝑠)，陀螺仪输出角速度拉氏 变换为𝛺(𝑠)，输出为𝜃(𝑠)，可以画出下面系统框图。
互补滤波系统框图
经过一些处理，可以把积分项去掉，系统简化为角速度乘以比例系数𝜏再和 加速度计的角度叠加，通过一个低通滤波器后输出。系统框图简化如下：
可以看出，经过简化后少了纯积分环节，那么系统便不再具有积累误差。我们来推导一下低通滤波器的时域表达式。 设低通滤波器系统函数为： 𝐺1 (𝑠) = 1/(𝜏𝑠 + 1)
设输入为𝑥(𝑠)，输出为𝑦(𝑠)，那么有：𝑦(𝑠) = 𝐺1 (𝑠)𝑥(𝑠) = [1/(𝜏𝑠 + 1)]𝑥(𝑠)
作反变换可得时域表达式为：
因便于计算机程序处理，我们作离散化处理并简化，可以得到离散时域表达式如下：
这就是低通滤波器的时域表达式，在我们的互补滤波和速度环里面都有应用。同样的道理，互补滤波中，角速度乘以比例系数τ再和加速度计的角度叠加，经过低通滤波器后时域表达式为：
对应我们的代码为：
angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * dt);可以看出，互补滤波就是通过加速度计获取的角度对陀螺仪积分的角度进行校准，从而积分的角度逐步跟踪到加速度传感器所得到的角度。K1 与 1-K1 是对这两个角度取不同的权重，可以表示我们对不同数据的信任程度。
互补滤波算法
进行滤波前需要先读取原始的角速度，并且读出加速度计数据用于计算角度。 MPU6050 的数据读取和写入均通过其内部的寄存器来实现，这些寄存器都是 1 个字节，即 8 位。角速度和加速度的数据分为高 8 位与低 8 位，分别用 2 个寄存器储存，读取数据后应把他们整合在一起，如读取 X 轴角速度数据，代码如下：
Gyro_X=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_H<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_L); //读取 X 轴陀螺仪，高 8 位与低 8 位整合 MPU6050 采用 16 位有符号数作为陀螺仪测量数据输出，所以最小的数为 -32767，最大的数为 32767。我们初始化了陀螺仪的最大量程为±2000°/s，所以 数字 32767 对应 2000°/s，同理负数也是如此。用 32767 除以 2000 即可以得到灵敏度为 16.4，即每度多少个读数，官方手册也给出了各个量程的灵敏度。把陀螺仪读出的数字除以灵敏度就可以转换为角速度(°/s)。在此之前，我们需要作一些处理，把读数转换成有符号类型的数，就像读取编码器一样，代码如下：
if(Gyro_X>32768) Gyro_X-=65536; //数据类型转换 也可通过 short 强制类型转换 读数转换为角速度(°/s)，以 X 轴为例，代码如下：
Gyro_X=Gyro_X/16.4; //陀螺仪量程转换，灵敏度 16.4 已经得到了角速度，那么使用加速度计的数据计算出角度就可以使用滤波算法了。在前面已经说明了角度计算公式。X 轴与 Y 轴角度计算代码如下：
Accel_Angle_x=atan2(Accel_Y,Accel_Z)*180/PI; //计算倾角并转换单位
Accel_Angle_y=atan2(Accel_X,Accel_Z)*180/PI; //计算倾角并转换单位 用公式计算出来的角度为弧度制，需要把角度与角速度的单位统一起来，一律使用度作单位，所以最后作了单位转换。互补滤波程序如下，入口参数为角度与角速度。
float First_order_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;
}卡尔曼滤波
卡尔曼滤波介绍
传感器测量的数据总是有很多的不确定性，比如有很多的噪声，而这些噪声大部分都符合高斯分布。对于我们的小车，输入是角速度，输出是角度，这是一个线性的系统。如果一个系统是线性的系统，而且这些不确定性是符合高斯分布的，那么我们就可以使用卡尔曼滤波算法进行最优估计。卡尔曼滤波的思想就是使用系统的状态方程预测当前的值，使用传感器测出来的观测值来修正这个预测值。与互补滤波一样，可以选择不同的权重来实现，但是这个权重是动态变化的。
我们知道一个系统的状态方程和传感器的测量方程如下：
其中，𝑋𝑘 和𝑍𝑘 分别表示系统的状态矩阵和系统的观测量矩阵，𝑈𝑘−1是系统的输入量，A 和 B 是系统参数，一般为矩阵的形式。H 为观测矩阵，表示把实 际量转换为观测量的一个参数。在实际过程中，会掺杂有噪声，𝑊𝑘−1和𝑉𝑘 分别表示过程噪声和观测噪声，他们服从高斯分布，即 W~N(0,Q),V~N(0,R)，Q 和 R 是一个协方差矩阵。卡尔曼滤波可以分为两大步骤，分别叫做预测和更新。下面给出卡尔曼的公式：
上面公式中，上标带负号的表示根据状态方程计算出来的数值，是理想的出，也叫先验估计，后面需要对这个值进行修正。上面带尖的是系统某时刻的最优估计，是滤波后的输出，也叫后验估计。Q 是系统过程噪声协方差，R 为测量噪声协方差，𝐾𝑘 是卡尔曼增益，用于修正先验估计，𝑃 𝑘 表示 k 时刻后验估计协方差矩阵，𝑃𝑘− 表示先验估计协方差矩阵，𝐼是单位矩阵。
所以我们就得到了小车的系统参数矩阵 A、B 和观测矩阵 H。接下来就可以 开始计算迭代，进行最优估计。
首先进行先验估计，即对应公式的预测部分：
因为为𝐴𝑛𝑔𝑙𝑒和𝐺𝑦𝑟𝑜_𝑏𝑖𝑎𝑠是相互独立的，故𝑐𝑜𝑣(𝐴𝑛𝑔𝑙𝑒, 𝐺𝑦𝑟𝑜_𝑏𝑖𝑎𝑠) = 𝑐𝑜𝑣(𝐺𝑦𝑟𝑜_𝑏𝑖𝑎𝑠, 𝐴𝑛𝑔𝑙𝑒) = 0。可以看到我们的协方差矩阵最终是由加速度算出的
角度的方差和陀螺仪的角速度方差组成，矩阵后面乘了一个𝑑𝑡，这是因为𝑄 𝑘是与 k 时刻的时间相关的，当距离上次更新时间越长，那么这个方差也会越大，就比 如陀螺仪积分出来的角度，时间越长，那么偏差累积也会越大。把参数代入，可得协方差矩阵先验估计如下：
这个公式可以很容易在程序中实现。
通过一次次的迭代，最终使输出的角度平滑准确。
核心代码讲解
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; 
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;
完整代码
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;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;
}