互补滤波姿态估计程序

/**
 * 6DOF 互补滤波姿态估计(via Mahony)
 * @param[in] halfT:状态估计周期的一半
 */
const float Kp = 3.5, Ki = 0.05;
float exInt, eyInt, ezInt;
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // roll,pitch,yaw 都为 0 时对应的四元数值。
void imuEstimatedAttitude(float gx, float gy, float gz, float ax, float ay, float az, float halfT) 
{
    float norm;
	float vx, vy, vz;  // v-陀螺仪积分后推算出的重力向量
	float ex, ey, ez;  // e-两重力向量的叉积（向量间的误差 a与v之间）   
	
	// 加速度原始数据归一化，得到单位化的加速度。
	norm = sqrt(ax*ax + ay*ay + az*az);
	ax = ax/norm;
	ay = ay/norm;
	az = az/norm;
	 
	// 坐标系变换，将地理坐标系“R系”的重力向量转换到机体坐标系“b系”的v_xyz
	vx = 2*(q1*q3 - q0*q2);
	vy = 2*(q0*q1 + q2*q3);
	vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
	
	// a_xyz是测量得到的重力向量，v_xyz是陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量。
	// 那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。
    // 这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，
	// 而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺仪积分误差。
	ex = (ay*vz - az*vy);
	ey = (az*vx - ax*vz);
	ez = (ax*vy - ay*vx);
	
	// 误差作PI修正
	exInt = exInt + ex*Ki;
	eyInt = eyInt + ey*Ki;
	ezInt = ezInt + ez*Ki;

	gx = gx + Kp*ex +exInt;
	gy = gy + Kp*ey +eyInt;
	gz = gz + Kp*ez +ezInt;
	
	// 四元数微分方程求解
	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;

	norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
	q0 = q0/norm;
	q1 = q1/norm;
	q2 = q2/norm;
	q3 = q3/norm;

	roll =  atan2f(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1)*57.3;    	
	pitch =  asinf(2*q1*q3 - 2*q0*q2)*57.3;                                                          
	yaw  =  -atan2f(2*q1*q2 + 2*q0*q3, -2*q2*q2 -2*q3*q3 + 1)*57.3; 
}