效果：
MPU6050姿态解算-卡尔曼滤波+四元数+互补滤波
目录
[2. 预测协方差方程](#2. 预测协方差方程)
[3. 卡尔曼增益方程](#3. 卡尔曼增益方程)
[4. 跟新最优值方程（卡尔曼滤波的输出）](#4. 跟新最优值方程（卡尔曼滤波的输出）)
[5. 更新协方差方程](#5. 更新协方差方程)
基础知识详解
欧拉角
欧拉角：以运动物体建立坐标系，通过绕轴旋转来表示运动物体当前的姿态( Roll - 翻滚角，Pitch - 俯仰角，Yaw - 偏航角）绕着Z轴旋转过的角度叫做yaw偏航角，绕着X轴旋转过的角度叫做roll翻滚角，绕着Y轴旋转过的角度叫做yaw俯仰角。接下来细致讲解去理解这三个角（欧拉角）
可以先考虑一个问题 ：比如我们想要在空间中去表示任意一个点的位置，那这个时候我们首先需要建立一个坐标系，需要一个三轴的直角坐标系，就可以使用一个坐标xyz表示空间中任意一个点的位置。
今天所学习的欧拉角的坐标系与我们传统意义上的直角坐标系是有所区别的 ，对于传统直角坐标系的位置是固定不变的，而欧拉角的直角坐标系会随着物体的旋转和运动一起去运动，如下的一个球，以这个球建立一个坐标系，一开始球的三个轴与XYZ轴是重合的（图1）。首先沿Z轴旋转（图2），再沿X轴旋转（图3），再沿Y轴旋转（图4），经过这三次旋转之后就可以得到空间中的任意姿态，所以就可以使用这三次旋转的角度去表示球的运动姿态，这三个角度就是Roll - 翻滚角，Pitch - 俯仰角，Yaw - 偏航角，也就是所述的欧拉角。
图1 球的三个轴与XYZ轴重合
图2 球沿Z轴旋转
图3 球沿X轴旋转
图4.球沿Y轴旋转
形象理解：如下图，以飞机本身为参考系建立坐标系，这个坐标系会随着飞机的旋转和运动一起去运动。绕着Z轴旋转的角度叫做yaw偏航角，绕着X轴旋转的角度叫做roll翻滚角，绕着Y轴旋转的角度叫做yaw俯仰角。
绕着Z轴旋转的角度决定飞机在水平方向上的一个朝向，也就是决定了飞机的航向， 比如让机头绕着Z轴旋转一个角度，这个时候的夹角就是yaw偏航角，航向就发生了变化
翻滚角是绕着X轴旋转过的角度，当飞机绕着X轴旋转的时候，就像人在床上打滚的一个动作，所以很形象的把这个角叫做翻滚角
俯仰角就是绕着Y轴旋转过的角度，如果绕着y轴逆时针去旋转，飞机就在点头，绕着y轴顺时针旋转的时候，飞机就在抬头，那一点头一抬头就形象叫做俯仰。
在实际的编码过程中，就是使用欧拉角去表示物体的运动姿态
测量欧拉角的数值有两种方式：1.加速度计 2.陀螺仪。对于这两种方式来说测量欧拉角的结果互有优缺点，所以需要将这两种方式测得的欧拉角结果进行融合来得到更加精确的欧拉角的值
接下来理解一下加速度计和陀螺仪是如何测量欧拉角的
加速度计(Accelerometer)与姿态测量
加速度计就是用来测量加速度的一个传感器，对于mpu6050来说可以测量三个轴向的加速度，分别是沿X轴，y轴，z轴的加速度。有了这三个轴向的加速度后，就可以来计算欧拉角
举飞机例来形象理解三个轴向的加速度
如下图，当前飞机的姿态是斜向上的，假设飞机当前是匀速直线运动或静止，那么对于飞机只受一个力也就是重力，重力产生的加速度就是重力加速度g（9.8米每秒方）方向是垂直于地面向下所以可以以它为参考来计算欧拉角。
计算欧拉角之前，首先沿着三个轴向对重力加速度g进行分解就会得到三个轴向的加速度也就是ax，ay，az（ax，ay，az就是加速度计在三个轴向上的一个读数）
图5 飞机模型图
接着就可以使用这三个ax，ay，az数来计算欧拉角。
需要注意 使用加速度计来计算欧拉角只能算出roll翻滚角和pitch俯仰角，当飞机的方向大体处于水平方向时是没有办法通过加速度计的方式去计算yaw偏航角
接下来形象理解roll翻滚角和pitch俯仰角的计算
形象理解roll翻滚角的计算
在计算之前，我们形象来逆着图5x轴方向看（飞机尾部向里看）如下所示的主视图。在主视图中粉红色的线是重力加速度g，右边红色箭头是az重力加速度g在z轴上的一个分量，左边红色箭头是ay就是重力加速度g在y轴上的一个分量。
在如下主视图中，飞机是在逆时针旋转了一个角度α也就是翻滚角roll，夹角α的对边就是ay，临边是az，所以可以通过三角关系来求出夹角α也就是翻滚角roll，通过
来计算出翻滚角roll
形象理解pitch俯仰角的计算
接着看一下右视图（逆着图5y轴往左看），在右视图中粉红色箭头表示重力加速度g，角度β就是pitch俯仰角的相反数也就是-pitch（因为规定pitch是绕着图5y轴逆时针旋转过的角度，而β角是顺时针旋转的，所以要取一个负号）。
β角的对边是ax，领边是az，所以使用三角关系
就得到了β角也就是俯仰角
加速度计测量方式的优缺点
缺点：外部震动会引入噪声（白噪声）。因为在实际的情况下，运动的物体可能处于变速运动，而且也可能受到周围各个方向上的力从而使物体产生震动，如果产生震动其实就是一种变速运动，变速运动就会有加速度所以会产生各个方向上随机的加速度，那反应在波形上如下所示是x轴上的加速度的实测的曲线，在曲线上就会看到各种各样的毛刺，这些毛刺就是周围震动产生的影响，毛刺的出现是随机的而且幅度大小也随机所以类似于一种白噪声，所以只使用重力加速度去计算欧拉角是不够的，需要引入其它的测量方法
接下来讲解理解一下陀螺仪的测量物体运动的姿态
陀螺仪(Gyroscope)与姿态测量
陀螺仪就是使用角动量守恒物理定律所制作的一种传感器用来测量角速度。陀螺仪能够测量绕轴旋转的角速度，单位°/s
对于mpu6050可以输出三个轴向的角速度，分别是gx，gy，gz。gx就是飞机绕着x轴逆时针旋转的角速度，gy就是绕着y轴逆时针旋转的角速度，gz就是绕着z轴旋转的角速度。例如从mpu6050传感器中读出gx的值是720，也就是意味着飞机模型每秒会绕着x轴旋转两周。
有了角速度后可以进行计算欧拉角， 接下来理解一下计算过程
以pitch俯仰角为例理解角速度计算欧拉角
我们知道pitch俯仰角是绕y轴旋转过的角度，gy是绕y轴逆时针旋转的角速度，所以gy就是pitch俯仰角的一个导数（角速度等于角度关于时间的导数，不理解的看下方灰色块补充处），对pitch俯仰角求导就得到了gy。如下图，假设灰色线是pitch俯仰角的一个曲线，gy就代表曲线上的切线也就是曲线的斜率
如下，在t0时刻开始经过∆t时间到t1新时刻的pitch俯仰角=t0时刻的pitch俯仰角+gy乘∆t（∆t时间很短所以在∆t时间内曲线的斜率保持不变，又角速度等于角度关于时间的导数，所以gy乘∆t就等于∆t时间的pitch俯仰角）
综上，把这个规律总结为一个公式，新角度=上次角度+角速度x间隔时间 ，进而就求出了欧拉角
所以就有了下面三个公式，其中t表示新角度的时间，新角度时间t减去∆t就是上次角度的时间
陀螺仪计算欧拉角的优缺点
优点：准确、无干扰。因为陀螺仪是使用角动量守恒原理来制作的传感器，因此不会受到外部震动的影响，所以短期的测量结果是非常准确的
缺点：误差会累积。因为可以从公式中得知新角度是使用上一次的角度作为依据进行计算的，而新计算的角度在下一次的运算过程中又会被作为计算更新角度的计算依据，所以计算过程是一个迭代的过程，若在迭代的过程中某一次的运算产生了误差，那么这个误差就会被累积下去，当不断累积这个误差时我们计算的欧拉角的值就会跟实际的值产生偏移，这个就是误差累积。
如下图所示是实测的一个曲线，其中粉红色曲线是实测的gy值也就是绕y轴旋转的角速度，黄色线是实测pitch 俯仰角的值，红色线是我们通过上面公式所计算出的新角度pitch 俯仰角的值。可以看到刚开始时计算的俯仰角和实测 俯仰角是重合的，然后随着时间的延长，这个误差会逐渐累积，计算值和实测值之间的差距会越来越大，最终会完全偏离实际值
补充***理解角速度等于角度关于时间的导数***
角速度(ω)表示每单位时间角度的变化率,其单位是rad/s。
角度(θ)表示物体在转动过程中转过的角大小,其单位是rad(弧度)。
两者之间的关系为:ω = dθ/dt 其中,dθ是角度θ在时间dt内的变化量。这表示角速度ω等于角度θ关于时间t的导数。
通过这个关系式,可以推导出:θ = ∫ωdt 即角度等于角速度ω关于时间t的积分。
综上：
如果给出角度θ与时间t的函数,可以通过求导获得角速度ω与时间t的关系。
如果给定角速度ω随时间t的函数表达式,可以通过积分求出角度θ与时间t的关系。
总结：
加速度计和陀螺仪的优缺点正好相反，是可以互相补足的，加速度计时使用反正切值直接推导出的欧拉角所以长期来看不存在漂移，陀螺仪是使用迭代的方式进行计算的所以误差会累积即长期会漂移严重
综上，接下来将加速度计和陀螺仪这两个测量结果结合起来从而得到一个更精确的欧拉角
姿态解算算法1-互补滤波
数据融合，首先计算陀螺仪和加速度计的欧拉角测量结果，然后将这两个结果前面都乘一个系数权重相加就得到了融合之后的最终结果。
陀螺仪前面权重是α，加速度计前面权重是1-α，例如α=0.98，加速度计的权重就是0.02。在实际的使用过程中可以微调α的值来让融合之后获得更好的性能。
数据融合具体计算过程如下：
通过加速度计来计算出欧拉角中roll翻滚角和pitch俯仰角的值
通过陀螺仪读出三轴的角速度gx，gy，gz，就可以使用原来的角度+角速度乘以∆t算出新的角度，这样就通过陀螺仪计算出了三个欧拉角Roll翻滚角，Pitch俯仰角，Yaw偏航角
然后对两组测量欧拉角的结果进行融合，通过陀螺仪和加速度计的欧拉角测量结果，将这两个结果前面都乘一个系数权重相加就得到了融合之后的欧拉角的最终结果。
权重α的取值问题
若α的值越大就代表越信任陀螺仪的测量结果，反之则代表越信任加速度计的测量结果，α不易取太大也不易取太小，α的取值可以用下面这个公式，其中∆t代表读取传感器的时间间隔（例如以200Hz的频率去读数取mpu6050的测量结果，这时的读取的间隔就是5ms，这个不理解的可以看下面的灰色块补充处理解）
接下来讲一下关于上面公式中的τ的含义
下图中紫色线表示使用陀螺仪计算的一个结果，红色线表示使用加速度计计算的结果，粉红色表示融合后的结果。
可以看到对于陀螺仪计算的结果的变化趋势是对的，但是和真实值（红色线）之间存在着较大的偏移，对于红色线来说大体的趋势是对的但是有一些毛刺噪声，对两组数据进行融合得到粉红色的曲线，粉红色线就比较好的反应了运动姿态
τ值就等于加速度计对陀螺仪纠偏移的一个时间，τ时间越短代表收敛的速度就越快，就越接近于加速度计的测量结果，τ的时间越长代表收敛的速度就越慢，就越接近于陀螺仪的测量结果，所以τ要取一个合适的值，一般τ取值为0.1s。
补充***200Hz是5ms ***
每秒二百次，每次二百分之一秒，二百分之一秒就是0.005秒。一秒等于一千毫秒，所以一个周期就是五毫秒，五毫秒还等于五千微秒
姿态解算算法2-四元数法
四元数法 (又称四参数法) 。英国数学家W.R.Haminlton 在1843 年在数学中引入了四元数。但直到20 世纪60 年代末期这种方法还没有得到实际应用，随着空间技术、计算技术SINS 技术的发展，四元数才引起人们的重视。求解四元数微分方程要解四个微分方程。虽然要比解欧拉微分方程多一个方程，但其优越性在于计算量小、精度高、可避免奇异性，该方法是目前研究的重点之一。由于方向余弦法在对载体姿态动力学求解时会产生歪斜、刻度和漂移误差等，然而，SINS 中在进行姿态求解时估计出这些误差是很重要的。与方向余弦法相比，四元数法的优点在于不仅歪斜误差等于零；而且刻度误差的推导很简单，能得出便于进一步分析的解析表达式，而方向余弦法只有在特殊的情况下才能分析和检测到刻度误差，且不能得出通用的结论。通过从不同角度对欧拉角法、方向余弦法和四元数法进行对比。结果表明四元数法具有最佳的性能。
顾名思义，四元数是由四个元构成的数。
其中，q0、q1、q2、q3是实数，i、j、k即使互相正交的单位向量，又是虚单位向量
四元数详细推到余请看参考链接： 无人机四元数解算姿态角解析
姿态解算算法3-卡尔曼滤波
采用递归的方法解决线性滤波问题，只需要当前的测量值和前一个采样周期的估计值就能进行状态估计，需要的存储空间小，每一步的计算量小。
如下蓝色的波形是实际测得的数据，红色的波形是经 Kalman 滤波后的数据波形。
注：这里是实际应用激光测距传感器（TOF）vl53l0x 测得的距离数据。
组成
1.预测状态方程
由 系统状态变量k-1时刻的最优值 和 系统输入 计算出k时刻的 系统预测值。
①. X k-1|k-1 为k-1时刻的输出。
②. 当X为一维数据时，Fk的值是1。
③. 一维数据下（uk=0时）：系统预测值 = 系统状态变量k-1时刻的最优值。
2. 预测协方差方程
根据 k-1时刻的系统协方差 预测 k时刻系统协方差。
①. 当X为一维数据时，Fk的值是1。
3. 卡尔曼增益方程
根据（k时刻） 协方差矩阵的预测值 计算 卡尔曼增益。
①. 当 Pk|k-1 为一个一维矩阵时，Hk 是1。
4. 跟新最优值方程（卡尔曼滤波的输出）
根据 状态变量的预测值 和 系统测量值 计算出 k时刻状态变量的最优值。
①. 当 Pk|k-1 为一个一维矩阵时，Hk 是1。
5. 更新协方差方程
为了求 k时刻的协方差矩阵。（为得到k+1时刻的卡尔曼输出值做准备）
①. 当 Pk|k-1 为一个一维矩阵时，Hk 是1。
参考链接：卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用_卡尔曼滤波算法c语言-CSDN博客
MPU6050简介
mpu6050会测量角速度、加速度、温度三种不同的物理量，芯片内部有陀螺仪、加速度计、温度计，这三个传感器测量的模拟量经过AD转换成数字信号，得到gx，gy，gz，ax，ay，az，t温度，这些物理量会进入DMP进行运算，DMP是可编程的器件，可以将程序下载到DMP里对传入的数据进行处理（一般情况下不用），然后通过I2C接口去配置芯片的一些参数和读出测量结果。
阅读产品说明书和寄存器手册可参考进行编程，会放在资料包，后面会讲编程
需要注意，MPU6050没有磁力计，所以偏航角会有漂移。建议使用四元数法的程序
硬件连接
接五个引脚到主控芯片，分别是VCC、GND、SCL（主i2c时钟）、SDA（主i2c数据）和AD0（使用软件I2C可以不接此引脚）。
其中VCC接3.3v。SCL和SDA可查阅主控芯片的数据手册了解引脚。同时SCL和SDA需要外接上拉电阻4.7k（因为I2c接口使用的是开漏输出模式）。GND接电源地。AD0是地址选择引脚（mpu6050为从机），接电源地GND。
MPU6050剩余的XDA、XCL和INT引脚悬空，不接线。XDA和XCL引脚用于将6轴传感器拓展到9轴（接磁力计）INT是中断引脚，每当一个Sample的数据采集完成后发送脉冲
在数据手册中可查找到I2C的SCL和SDA引脚 。
编程实现-源码下载
包含完整3个算法源码+工程文件+上位机软件+模块参考资料
https://download.csdn.net/download/m0_61712829/89087094?spm=1001.2014.3001.5501