代码解析（超详细！！！）

这部分关于姿态解算的代码解析有一部分是参考别人的基础上注释的，不喜勿喷！！！

gyro.x = gyro.x * DEG2RAD;	//角度换成弧度
gyro.y = gyro.y * DEG2RAD;  
gyro.z = gyro.z * DEG2RAD;

//大部分的传感器输出的角速度是“度每秒”，但是实际上，角速度的国际单位是“弧度每秒”，这点是一定要注意的，当你直接拿着传感器的原始数据进行算法实现的时候，会发现输出的四元数就不稳定，一直在变化；

//这个部分是有关加速度矫正角速度的代码
	normalise = invSqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
	acc.x *= normalise;
	acc.y *= normalise;
	acc.z *= normalise;

在多指标评价体系中，由于各评价指标的性质不同，通常具有不同的量纲和数量级。当各指标间的水平相差很大时，如果直接用原始指标值进行分析，就会突出数值较高的指标在综合分析中的作用，相对削弱数值水平较低指标的作用。因此，为了保证结果的可靠性，需要对原始指标数据进行标准化处理。

// 第二步：DCM矩阵旋转
//载体坐标系到导航（机体）坐标系的转换
 vx = 2*(q1q3 - q0q2); 
 vy = 2*(q0q1 + q2q3); 
 vz = q0q0 - q1q1 - q2q2 + q3q3

但是如果写为矩阵与向量相乘就很清楚了，即A()=C*B，
A为转化完的加速度（导航坐标系）
b到n是机体坐标系到载体坐标系的转化矩阵
而最终A是从载体坐标系到机体坐标系的转化矩阵，这个转化矩阵是c
B是标准的重力加速度（0,0,1），使用标准的重力加速度是因为在机体运行过程中，在世界坐标系下（即以人的角度去观察机体坐标系的话），只受到垂直向下（Z轴）的力的作用，这个力就是重力，加速度为9.8m/s/s,而xy轴不受到力的作用，所以数据是0。
（因为举个例子，飞机在运行过程当中是在地球表面飞的，只受到竖直向下的力，为0，0，g，也就是加速度计在载体坐标系下读取到的原始数据，加速度传感器实际上是用MEMS技术检测惯性力造成的微小形变，把加速度传感器水平静止放在桌子上，它的Z轴输出的是1g的加速度。因为它Z轴方向被重力向下拉出了一个形变。）

ex = (acc.y * rMat[2][2] - acc.z * rMat[2][1]);
ey = (acc.z * rMat[2][0] - acc.x * rMat[2][2]);
ez = (acc.x * rMat[2][1] - acc.y * rMat[2][0]);

对机体加速度与标准加速度的映射进行向量的叉乘，得到补偿量，这一步可以说是本算法中最精髓的部分，我们都知道，向量具有点乘和叉乘两种运算，点乘公式为A·B=|A||B|*cosθ，叉乘的公式为A×B=|A||B|*sinθ,其中θ为两个向量之间的夹角，这里只说在本算法中，A和B是两个单位向量，因此其模为1，所以这样的公式下来就只剩下sinθ了，而在数学上，当θ极小的时候，sinθ是等于θ的，因此这里的公式走下来之后就得到了机体坐标系下实际加速度向量与理想加速度向量之间的差角，
rmat是上一次的加速度值。（机体坐标系）
ex，ey，ez是在x，y，z方向上的误差，也就是角度

exInt += Ki * ex * dt ;  
eyInt += Ki * ey * dt ;
ezInt += Ki * ez * dt ;
	
gyro.x += Kp * ex + exInt;
gyro.y += Kp * ey + eyInt;
gyro.z += Kp * ez + ezInt;

相当于是pid当中的p、i运算，这里开始用加速度修正角速度的操作这里要特别解释一下，假如我们没有修正角速度，则四元数会因为角速度的漂移而漂移，因此得到的DCM矩阵也会漂移，这时候标准重力加速度在机体坐标系下的映射也会漂移，但是加速度计的数据是长期稳定的，它会与漂移之后的映射不重合，两个向量之间就会产生角度差（由于时间很短，认为是角速度），使用这个角度差矫正角速度数据，就会使抑制角速度长期不稳定，得到准确的四元数。PI运算，这里特别强调一下，因为飞行器在飞行过程中机架的震动很容易传到传感器上，加速度又因为短期不稳定，十分容易受到这样震动的影响，变得不稳定，所以在矫正角速度的时候，PI值不易过大，一般P值小于1，I值小于0.01，当然，如果防震措施较好，则可以适当大一些。

//使用gyro三个角速度以及老的四元数计算出一个新的四元数
float q0Last = q0;
float q1Last = q1;
float q2Last = q2;
float q3Last = q3;
q0 += (-q1Last * gyro.x - q2Last * gyro.y - q3Last * gyro.z) * halfT;
q1 += ( q0Last * gyro.x + q2Last * gyro.z - q3Last * gyro.y) * halfT;
q2 += ( q0Last * gyro.y - q1Last * gyro.z + q3Last * gyro.x) * halfT;
q3 += ( q0Last * gyro.z + q1Last * gyro.y - q2Last * gyro.x) * halfT;

//单位化四元数
normalise = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= normalise;
q1 *= normalise;
q2 *= normalise;
00q3 *= normalise;

//计算四元数的旋转矩阵
imuComputeRotationMatrix();	
//最终得到欧拉角
state->attitude.pitch = -asinf(rMat[2][0]) * RAD2DEG; 
state->attitude.roll = atan2f(rMat[2][1], rMat[2][2]) * RAD2DEG;
state->attitude.yaw = atan2f(rMat[1][0], rMat[0][0]) * RAD2DEG;

源代码

void imuUpdate(Axis3f acc, Axis3f gyro, state_t *state , float dt)	
{
	float normalise;
	float ex, ey, ez;
	float halfT = 0.5f * dt;
	float accBuf[3] = {0.f};
	Axis3f tempacc = acc;
	
	gyro.x = gyro.x * DEG2RAD;	
	gyro.y = gyro.y * DEG2RAD;
	gyro.z = gyro.z * DEG2RAD;

	if((acc.x != 0.0f) || (acc.y != 0.0f) || (acc.z != 0.0f))
	{
		normalise = invSqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
		acc.x *= normalise;
		acc.y *= normalise;
		acc.z *= normalise;

		ex = (acc.y * rMat[2][2] - acc.z * rMat[2][1]);
		ey = (acc.z * rMat[2][0] - acc.x * rMat[2][2]);
		ez = (acc.x * rMat[2][1] - acc.y * rMat[2][0]);
		
		exInt += Ki * ex * dt ;  
		eyInt += Ki * ey * dt ;
		ezInt += Ki * ez * dt ;
		
		gyro.x += Kp * ex + exInt;
		gyro.y += Kp * ey + eyInt;
		gyro.z += Kp * ez + ezInt;
	}
	float q0Last = q0;
	float q1Last = q1;
	float q2Last = q2;
	float q3Last = q3;
	q0 += (-q1Last * gyro.x - q2Last * gyro.y - q3Last * gyro.z) * halfT;
	q1 += ( q0Last * gyro.x + q2Last * gyro.z - q3Last * gyro.y) * halfT;
	q2 += ( q0Last * gyro.y - q1Last * gyro.z + q3Last * gyro.x) * halfT;
	q3 += ( q0Last * gyro.z + q1Last * gyro.y - q2Last * gyro.x) * halfT;
	
	normalise = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= normalise;
	q1 *= normalise;
	q2 *= normalise;
	q3 *= normalise;
	
	imuComputeRotationMatrix();	
	

	state->attitude.pitch = -asinf(rMat[2][0]) * RAD2DEG; 
	state->attitude.roll = atan2f(rMat[2][1], rMat[2][2]) * RAD2DEG;
	state->attitude.yaw = atan2f(rMat[1][0], rMat[0][0]) * RAD2DEG;
	
}

*好了，以上就是我对基于互补滤波器的姿态解算的理解，可能会有理解不对的地方，如果有，希望大家批评指正，谢谢！！！