接着上篇文章：
https://mp.csdn.net/postedit/101777018
一般在程序中，姿态解算的方式有两种：一种是欧拉角法，一种是四元数法。
这里不介绍欧拉角法，只介绍四元数法。
互补滤波算法：
顾名思义，是多组数据结合互补，并进行滤波处理稳定输出，得到姿态的算法。而我们使用的传感器就是加速度计和陀螺仪。加速度计用于测量加速度，陀螺仪用于测量角速度。 加速度计的静态稳定性更好，而在运动时其数据相对不可靠；陀螺仪的动态稳定性更好，但是静止时数据相对不可靠。所以，我们可以通过加速度计的输出来修正陀螺仪的漂移误差，换句话说，通过加速度计来修正陀螺仪。
首先，我们取定导航坐标系n中标准重力加速度g，定义为
，那么将导航坐标系n下的
转换为载体坐标系b下的： 。
这里用到了
，前面在推导基础公式时导出了使用四元数表示的旋转矩阵
。
公式如下：
但是我们要用的是
，所以还要对 做一个矩阵逆变换。由于它是正交矩阵，对于正交矩阵有这个性质：正交矩阵的逆矩阵等于其的转置。所以我们很容易得到： 
将上式代入
，而且我们已知
，所以得到：
接着再定义载体坐标系b中加速度计输出为a，由于前面计算导航坐标系时我们采用的重力加速度是标准重力加速度，所以还需要对其进行归一化，才能继续运算。
设加速度计三个轴的值分别是ax，ay，az。
首先求模：
。
归一化：
根据框图的说明再整理一下前面得到的结果：
标准重力加速度
从n系转到b系中的矩阵表示：
b系下加速度计测量得到的加速度的矩阵表示：
（备注：这是归一化之后的值）
对
和
做向量叉乘，即可得到给陀螺仪的校正补偿值e。
表示成矩阵形式更为直观：
然后再使用PI控制器进行滤波，准确地说事消除漂移误差，只要存在误差控制器便会持续作用，直至误差为0。控制的效果取决于P和I参数，分别对应比例控制和积分控制的参数。
这里给出PI控制的公式：
是我们要负反馈给陀螺仪进行校正补偿的值，
是比例控制项，
是积分控制项，在程序中采用离散累加计算。关于PID控制理论的东西，这里不做赘述。
如框图中所写，接下来将前面得到的补偿值
加在陀螺仪输出的数据上进行校正。
跟着框图往下走：
将前面的陀螺仪数据通过四元数微分方程转换为四元数输出。
因为有几个地方我也没搞懂，所以就简单介绍一下四元数微分方程，详细步骤请查阅秦永元的惯性导航一书（第三篇 9.2.3节）：
由于载体的运动，四元数Q是变量，其参数可以表示成关于时间的函数。
使用四元数的三角形式：
为刚体瞬时绕轴转过的角度， 为归一化后的位置矢量。
设角速度为：
对四元数的三角表示形式求导：
因为
， 且 。
所以：
将上式表示成矩阵形式：
或者
上面的两个公式被称为四元数微分方程。利用陀螺仪的数据进行离散累积便可得到四元数的值，最后再转换成欧拉角形式输出即可。
下面也添加了一些注释，把互补滤波算法的理论部分过了一遍后再来对着代码读一遍，应该不会觉得有多难了吧。程序中就只是把前面推导的公式一个个用上去了而已，如果不懂原理直接使用也不会有太大问题。
程序中的积分运算是采用离散累积的方法运算的。PID控制器只是起到了一个稳定数据消除漂移误差的作用。
再附上代码：
#define Kp 10.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.008f // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.001f // half the sample period采样周期的一半
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
// float hx, hy, hz, bx, bz;
float vx, vy, vz;// wx, wy, wz;
float ex, ey, ez;
// 先把这些用得到的值算好
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
// float q0q3 = q0*q3;
float q1q1 = q1*q1;
// float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
if(ax*ay*az==0)
return;
norm = sqrt(ax*ax + ay*ay + az*az); //acc数据归一化
ax = ax /norm;
ay = ay / norm;
az = az / norm;
// estimated direction of gravity and flux (v and w)估计重力方向和流量/变迁
vx = 2*(q1q3 - q0q2); //四元素中xyz的表示
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) ; //向量外积在相减得到差分就是误差
ey = (az*vx - ax*vz) ;
ez = (ax*vy - ay*vx) ;
exInt = exInt + ex * Ki; //对误差进行积分
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt; //将误差PI后补偿到陀螺仪，即补偿零点漂移
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt; //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减
// integrate quaternion rate and normalise //四元素的微分方程
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
//Q_ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}
