MPU6050 获取角度理论推导(二)---6轴融合算法

接着上篇文章：

https://mp.csdn.net/postedit/101777018

• 姿态角解算---互补滤波算法及理论推导

 

一般在程序中，姿态解算的方式有两种：一种是欧拉角法，一种是四元数法。

这里不介绍欧拉角法，只介绍四元数法。

互补滤波算法：

顾名思义，是多组数据结合互补，并进行滤波处理稳定输出，得到姿态的算法。而我们使用的传感器就是加速度计和陀螺仪。加速度计用于测量加速度，陀螺仪用于测量角速度。 加速度计的静态稳定性更好，而在运动时其数据相对不可靠；陀螺仪的动态稳定性更好，但是静止时数据相对不可靠。所以，我们可以通过加速度计的输出来修正陀螺仪的漂移误差，换句话说，通过加速度计来修正陀螺仪。

首先，我们取定导航坐标系n中标准重力加速度g，定义为 ，那么将导航坐标系n下的  转换为载体坐标系b下的： 。 

这里用到了 ，前面在推导基础公式时导出了使用四元数表示的旋转矩阵 。

 

公式如下：

但是我们要用的是 ，所以还要对  做一个矩阵逆变换。由于它是正交矩阵，对于正交矩阵有这个性质：正交矩阵的逆矩阵等于其的转置。所以我们很容易得到： 

将上式代入  ，而且我们已知 ，所以得到：

接着再定义载体坐标系b中加速度计输出为a，由于前面计算导航坐标系时我们采用的重力加速度是标准重力加速度，所以还需要对其进行归一化，才能继续运算。 
设加速度计三个轴的值分别是ax，ay，az。 

首先求模：