用AHRS算法，融合HMC5883L的三轴磁通和mpu6050的加速度、角速率。 


--------------------------------- 
#define Kp 2.0f  // 比例增益支配收敛率accelerometer/magnetometer 
#define Ki 0.005f  // 积分增益执政速率陀螺仪的衔接gyroscopeases 
#define halfT 0.5f  // 采样周期的一半 

//--------------------------------------------------------------------------------------------------- 

float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计 

方向 
float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差 

//==================================================================================================== 
// Function 
//==================================================================================================== 

void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { 
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;           

// 测量正常化 
norm = sqrt(ax*ax + ay*ay + az*az);        
ax = ax / norm; 
ay = ay / norm; 
az = az / norm; 
norm = sqrt(mx*mx + my*my + mz*mz);           
mx = mx / norm; 
my = my / norm; 
mz = mz / norm;          

// 计算参考磁通方向 
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2); 
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1); 
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);          
bx = sqrt((hx*hx) + (hy*hy)); 
bz = hz;         

//估计方向的重力和磁通（V和W） 
vx = 2*(q1q3 - q0q2); 
vy = 2*(q0q1 + q2q3); 
vz = q0q0 - q1q1 - q2q2 + q3q3; 
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2); 
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3); 
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);   

// 错误是跨产品的总和之间的参考方向的领域和方向测量传感器 
ex = (ay*vz - az*vy) + (my*wz - mz*wy); 
ey = (az*vx - ax*vz) + (mz*wx - mx*wz); 
ez = (ax*vy - ay*vx) + (mx*wy - my*wx); 

// 积分误差比例积分增益 
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; 
} 




/*输入参数为加速度xyz原始数据，磁力计xyz原始数据，陀螺仪数据必须转换为弧度制*/