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; //v*當前姿態(tài)計算得來的重力在三軸上的分量
float ex, ey, ez;
// auxiliary variables to reduce number of repeated operations
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;
// normalise the measurements
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;
// compute reference direction of magnetic field
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;
// estimated direction of gravity and magnetic field (v and w)
//參考坐標n系轉(zhuǎn)化到載體坐標b系的用四元數(shù)表示的方向余弦矩陣第三列即是背蟆。
//處理后的重力分量
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
//處理后的mag粥庄，反向使用DCM得到
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);
// error is sum of cross product between reference direction of fields and direction measured by sensors 體現(xiàn)在加速計補償和磁力計補償，因為僅僅依靠加速計補償沒法修正Z軸的變差菲嘴，所以還需要通過磁力計來修正Z軸叭首。（公式28）谋梭。《四元數(shù)解算姿態(tài)完全解析及資料匯總》的作者把這部分理解錯了哑姚，不是什么叉積的差祭饭，而叉積的計算就是這樣的。計算方法是公式10叙量。
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);
// integral error scaled integral gain
exInt = exInt + ex*Ki* (1.0f / sampleFreq);
eyInt = eyInt + ey*Ki* (1.0f / sampleFreq);
ezInt = ezInt + ez*Ki* (1.0f / sampleFreq);
// adjusted gyroscope measurements
//將誤差PI后補償?shù)酵勇輧x倡蝙，即補償零點漂移。通過調(diào)節(jié)Kp绞佩、Ki兩個參數(shù)寺鸥，可以控制加速度計修正陀螺儀積分姿態(tài)的速度。（公式16和公式29）
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
/
