姿态解算

  姿态传感器读出加速度和角速度，而对一个系统的自动控制往往需要更加上层和贴近应用的的一个属性：角度。所以需要通过加速度和角速度进行数据融合转化得到姿态角度。
  以MPU6050为例，姿态解算有硬解算即DMP解算和软解算，配置传感器内部的运动处理器搭配DMP软件驱动库可进行DMP解算，根据IMU的特性在MCU等开发平台上进行一些移植就可以输出三个姿态角，软件解算则只关注将传感器原始数据进行算法融合，这里我们只讨论软件姿态解算。

互补滤波

  软件解算的核心原理是互补滤波，实际上单独通过角速度积分可以得出角度，单独通过加速度根据反切也能得到角度，但是这两种方式都有着自己的缺陷：通过角速度积分得到的角度因为积分会积累误差，所以准确度会随着积分时间的延长逐渐减少，而角加速度瞬间稳定性较差，通过角加速度得出的角度稳定时效果还可以，但是当物体运动时效果便不理想。互补滤波就是针对这种情况将两者结合起来，扬长补短。首先通过角速度的积分得出角度值，然后再用加速度对此值进行不断矫正，这样便可达到动态和长时间的情况所得角度值都比较准确的效果。

一阶互补滤波的简单C语言实现姿态解算

基本步骤：
1.根据反切公式由加速度求出角度
2.角速度积分求出角度，加积分限幅
3.求出两者误差对角速度积分进行补偿
4.调节积分参数和角度融合参数
注：
1.求得是哪一轴角度需要注意，取决于积分用的那一轴的角速度
2.仅适用于平衡车等简单场景，不适用于无人机飞控，因存在万向节死锁问题

#define PI_MATH		3.14159265358979f
#define GYRO_K 	  0.005 
#define ANGLE_K   0.02

float angle_get( float gyro_y,float gyro_z,float accel_x,float accel_z)
{ 
	float acc_angle_y,angle_err,angle_out;
	static  float gyro_integration;
    					
	acc_angle_y = atan2(accel_x,accel_z) * 180 / PI_MATH;  
    	
	gyro_integration += gyro_y * GYRO_K;

	if(acc_angle_y >= gyro_integration + 10) acc_angle_y = gyro_integration + 10 ;
	if(acc_angle_y <= gyro_integration - 10) acc_angle_y = gyro_integration - 10 ;
	 
	angle_err = acc_angle_y - gyro_integration;

	gyro_integration += angle_err * ANGLE_K;
	
	angle_out = gyro_integration;
	
	return   angle_out;	
}

四元数姿态解算

  IMU（惯性导航单元）包含三轴的加速度和三轴的角速度，通过每个轴的数据融合可实现姿态解算，姿态实质上是机体坐标系相对于地理坐标系的状态，表达姿态以地理坐标系为基准表达两者的转化关系。机体的姿态可以用向量在地理坐标系各轴夹角余弦（投影）来表示。三个向量在三个轴上的方向余弦可组成一个三阶矩阵。

  姿态有三种表达方式：欧拉角、余弦旋转矩阵和四元数，欧拉角虽然最符合人们的思维习惯，但是存在着万向节死锁问题（旋转中存在两轴重合的情况下就无法按规则正确到达目标位置），而且计算较慢，所以使用计算快、内存小的四元数进行姿态解算的过程，解算完成再生成可用的欧拉角。

  这里使用了四元数形式的PI控制思想的Mahony互补滤波算法，使用传感器读出的角速度求出四元数，提取四元数中的重力分量，使用机体坐标系上的重力分量和机体坐标系的向量进行差积运算得出误差，
对此重力（也就是加速度误差）进行积分和比例运算（PI）然后补偿到陀螺仪上修正角速度积分漂移误差。q为四元数，b为机体坐标系，R为地理坐标系，则地理坐标系到机体坐标系的转换为：

根据余弦矩阵形式的欧拉角旋转表达和四元数所表示的旋转关系之间的对等。可以用四元数求出欧拉角。四元数转换为欧拉角，即三维空间角度：

Mahony互补滤波算法C实现

笔者阅读过多种无人机飞控源码，Mahony互补滤波算法是姿态解算时最常用的，源码实现也大同小异。最后总结主要步骤为：
1、归一化重力加速度
2、使用各轴夹角余弦矩阵求出估计重力分量
3、估计重力方向与测量重力方向的叉积之和求出误差
4、积分误差反馈调节及比例反馈调节（互补滤波）
5、一阶龙格库塔求解四元数微分方程
6、归一化四元数
7、四元数转欧拉角

float my_sqrt(float number)
{
	long i;
	float x, y;
	const float f = 1.5F;
	x = number * 0.5F;
	y = number;
	i = * ( long * ) &y;
	i = 0x5f3759df - ( i >> 1 );

	y = * ( float * ) &i;
	y = y * ( f - ( x * y * y ) );
	y = y * ( f - ( x * y * y ) );
	return number * y;
}

#define Kp (80.0f)
#define Ki (50.0f)

void mahony_imu_update(float *gyro , float *acc*,float *angle*) 
{	
	float gx,gy,  gz,  ax,  ay,  az;
	
	gx = gyro[0];
	gy = gyro[1];
	gz = gyro[2];
	ax = acc[0];
	ay = acc[1];
	az = acc[2];
		
	static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
	static float exInt = 0, eyInt = 0, ezInt = 0;
	float norm;
	float vx, vy, vz;// wx, wy, wz;	
	float ex, ey, ez;

	float halfT =0.001f ;

	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
	{	
		// Normalise accelerometer measurement
		norm = my_sqrt(ax * ax + ay * ay + az * az);	 
		norm = 1/norm;
		
		ax *= norm;
		ay *= norm;
		az *= norm;		
	
		// Estimated direction of gravity and vector perpendicular to magnetic flux
		vx = (q1 * q3 - q0 * q2)*2;
		vy = (q0 * q1 + q2 * q3)*2;
		vz = q0 * q0 - q1*q1-q2*q2 + q3 * q3;
	
		// Error is sum of cross product between estimated and measured direction of gravity
		ex = (ay * vz - az * vy);
		ey = (az * vx - ax * vz);
		ez = (ax * vy - ay * vx);
	
		// Compute and apply integral feedback if enabled	
	    exInt = exInt + ex * Ki;
		eyInt = eyInt + ey * Ki;
		ezInt = ezInt + ez * Ki;
			
		// Apply proportional feedback
		gx += Kp*ex + exInt; 
		gy += Kp*ey + eyInt; 
		gz += Kp*ez + ezInt; 
	}
		
	// Integrate rate of change of quaternion
	
	q0 += (-q1 * gx - q2 * gy -q3 * gz)*halfT;
	q1 += (q0 * gx + q2 * gz - q3 * gy)*halfT;
	q2 += (q0 * gy - q1 * gz + q3 * gx)*halfT;
	q3 += (q0 * gz + q1 * gy - q2 * gx)*halfT; 
	
	// Normalise quaternion

	norm = my_sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);	
	norm = 1/norm;
	
	q0 *= norm;
	q1 *= norm;
	q2 *= norm;
	q3 *= norm;
			
	angle[PITCH]=asin(-2*q1*q3 + 2*q0*q2) * 57.3;
	angle[ROLL] = atan2(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1) * 57.3;  
    angle[YAW] = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1) * 57.3;  
}