IC解密Mahony的互补滤波法。此法简单有效，希望能给学习四轴飞行器的朋友们带来帮助。关于姿态解算和滤波的理论知识，推荐秦永元的两本书，一是《惯性导航》，目前已出到第二版了;二是《卡尔曼滤波与组合导航原理》。程序中的理论基础，可在书中寻找。

下面开始进入正题：

先定义Kp，Ki，以及halfT 。

Kp，Ki，控制加速度计修正陀螺仪积分姿态的速度

halfT ，姿态解算时间的一半。此处解算姿态速度为500HZ，因此halfT 为0.001

#define Kp 2.0f

#define Ki 0.002f

#define halfT 0.001f

初始化四元数

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;

定义姿态解算误差的积分

float exInt = 0, eyInt = 0, ezInt = 0;

以下为姿态解算函数。

参数gx，gy，gz分别对应三个轴的角速度，单位是弧度/秒;

参数ax，ay，az分别对应三个轴的加速度原始数据

由于加速度的噪声较大，此处应采用滤波后的数据

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)

{

float norm;

float vx, vy, vz;

float ex, ey, ez;

IC解密将加速度的原始数据，归一化，得到单位加速度

norm = sqrt(ax*ax + ay*ay + az*az);

ax = ax / norm;

ay = ay / norm;

az = az / norm;

把四元数换算成“方向余弦矩阵”中的第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。所以这里的vx、vy、vz，其实就是当前的机体坐标参照系上，换算出来的重力单位向量。(用表示机体姿态的四元数进行换算)