花了整整一天时间在地磁融合计算姿态上，看了很多论坛，发现都是同一篇文章，但是一直理解不了，后来发现就是这些笔者全部都是一字不差照搬照抄，而最初的博主又没有说清楚导致。我还是结合原文加上我自己通俗的理解来解释一下
首先给出x-IMU关于陀螺仪、加速度计、地磁计的融合代码：
void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float recipNorm;
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float hx, hy, bx, bz;
float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
float halfex, halfey, halfez;
float qa, qb, qc; `
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);
return;
}
// 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
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Reference direction of Earth's magnetic field
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
bx = sqrt(hx * hx + hy * hy);
bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
// Estimated direction of gravity and magnetic field
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
// Integrate rate of change of quaternion
gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm; }
相信有很多人已经理解了加速度计补偿陀螺仪漂移的原理，（其实我一点都不理解），不过这部分代码在x-IMU官网上已经给出，大家可以自行下载(http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/) （这个里面有c程序，里面有两个函数MahonyAHRSupdateIMU()和MahonyAHRSupdate()，前者是仅用加速度计补偿陀螺仪的漂移，后者即是融合地磁仪补偿陀螺仪的漂移），有能力的可以自行查看Sebastian O.H. Madgwick在2010年4月发表的一篇论文（An efficient orientation filter for inertial and inertial/magneticsensor arrays），可以发现x-IMU官网上的融合代码就是基于此篇论文。
花了一天时间研究地磁融合代码，总算弄明白了其地磁融合的原理。为了让大家理解Madgwick对地磁量的处理方式，我先从加速度计补偿开始说起。
首先，东北天坐标系我们称之为W系（世界坐标系），载体坐标系我们称之为r系（机体坐标系）。对于四元数法的姿态解算，我们求的就是四元数；旋转矩阵R（用于表示w系和r系的相对关系）中的元素本来应该是三角函数，这里由于我们四元数法，所以矩阵中的元素就成了四元数。所以我们的任务就是求解由四元数构成的方向余弦矩阵wCr（wCr表示从r系到w的转换矩阵，同理，rCw表示从w系到r的转换矩阵，他们的关系是转置）。
显然，上述矩阵是有误差存在的。对于一个确定的向量n，用不同的坐标系表示时，他们所表示的大小和方向一定是相同的。但是由于这两个坐标系的转换矩阵存在误差，那么当一个向量经过这么一个有误差存在的旋转矩阵变换后，在另一个坐标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。我们刚才说了，这个旋转矩阵的元素是四元数，这就是说我们修正的就是四元数，于是乎我们的姿态就这样被修正了，这才是姿态解算的原理。(这一段说的很好，是最基本的原理)
我们的姿态解算求的是四元数，我们是通过修正旋转矩阵中的四元数来达到姿态解算的目的，而不要以为通过加速度计和地磁计来修正姿态，加速度计和地磁计只是测量工具和载体，通过这两个器件表征旋转矩阵的误差存在，然后通过算法修正误差，修正四元数，修正姿态。
我们可以边看MahonyAHRSupdate这个函数边理解以下的内容。
在w系中，加速度计输出为
在r系中，加速度计的测量值为
（这里有个前提，就是机体必须处于静止状态） 现在
利用这个误差来修正旋转矩阵，于是乎，我们的四元数就在这样一个过程中被修正了。（实际上这种修正方法只把b系和n系的XOY平面重合起来，对于z轴旋转的偏航，加速度计无可奈何，稍后给详细讲解）但是，由于加速度计无法感知z轴上的旋转运动，所以还需要用地磁计来进一步补偿。
以上部分就是单独存在于MahonyAHRSupdateIMU这个函数内的内容，还算比较好理解
我们知道加速度计在静止时测量的是重力加速度，是有大小和方向的；同理，地磁计同样测量的是地球磁场的大小和方向，只不过这个方向不再是竖直向下，而是与x轴（或者y轴）呈一个角度，与z轴呈一个角度。（想象一下地磁场的磁感应线）记作
这里我们让x轴对准北边，所以by=0，即
（这里又有一个bug，为什么要用x轴对准北边，而不是y轴，不是说好是东北天坐标系么，一般都是东x、北y、天z啊？）（20170122补充，因为地磁计的坐标系和加速度的坐标系是不一样的，如下图是我们用的9250的datasheet中的说明，明白了吧）
倘若我们知道bx和bz的精确值，那么我们就可以采用和加速度计一样的修正方法来修正。只不过在加速度计中，我们在n系中的参考向量是
变成了地磁计的
如果我们知道bx和bz的精确值，那么我们就可以摆脱掉加速度计的补偿，直接用地磁计和陀螺仪进行姿态解算，但是你看过谁只有陀螺仪和地磁计进行姿态解算吗？没有，因为没人会去测量当地的地磁场相对于东北天坐标的夹角，也就是bx和bz。那么现在怎么办？（这一段也讲得很好，可以理解一下）
前面已经讲了，我们的姿态解算就是求解旋转矩阵，这个矩阵的作用就是将w系和r正确的转化直到重合。
现在我们假设旋转矩阵是经过加速度计校正后的矩阵，当某个确定的向量（r系中）经过这个矩阵旋转之后（到w系），这两个坐标系在XOY平面上重合，只是在z轴旋转上会存在一个偏航角的误差。下图表示的是经过nCb*旋转之后的b系和n系的相对关系。可以明显发现加速度计可以把b系通过四元数法从任意角度拉到与n系水平的位置上，这时，只剩下一个偏航角误差。这也是为什么加速度计无法误差修正偏航的原因。
到这里，就好说了。现在我们反过来从r系推往w系（注意是r系转换到w系）：设地磁计在r系中的输出为
经过r系到w系的旋转之后得到
在这个XOY平面上（w系），
的投影为hx2+hy2。显然，地磁计在XOY平面上（w系）的向量的大小必定相同，所以有bx2= hx2+hy2。而对于bz的处理，我们不做变动，令bz=hz即可。经过这样处理之后的
这个值再和b系中的地磁计输出
将加速度计没能做到的z轴上的旋转修正，通过地磁计在XOY平面上的地磁力相同原理，得到了修正。于是乎，pitch和roll通过加速度计修正，然后在这个基础之上（该地磁计补偿方法必须依靠加速度计修正提供一致的XOY平面，才会有bx2= hx2+hy2等式成立），yaw通过地磁计来补偿，最终得到了没有偏差的实时姿态（也就是由四元数组成的旋转矩阵）。
至此，我终于想通了所有的地方，虽然依然存在局部的一些漏洞，但是总体思路已经理清，接下去就是看详细看代码就行了。
以下为自己对代码的理解归纳
0、测量参数（r系）
角加速度：gx, gy, gz
线加速度 : ax, ay, az
地磁计: mx, my, mz
1、归一化[ax,ay,az]T和[mx,my,mz]T
2、将地磁计值[mx,my,mz]T旋转到w系，得到[hx,hy,hz]T，因为我们规定w系的x轴指向北，所以理应hy = 0，w系上的地磁计值理应为[bx,0,bz]T，所以我们令bx=sqrt(hx2+hy2)，bz = hz。
3、计算测量值和估计值之间的误差（包括加速度和磁场）
1）将w系中的重力加速度[0,0,1]T旋转到r系中，变成[vx,vy,vz]T，计算测量值[ax,ay,az]T与估计值[vx,vy,vz]T的误差。
2）将w系中的地磁向量[bx,0,bz]T旋转到r系中，变成[wx,wy,wz]T，计算测量值[mx,my,mz]T与估计值[wx,wy,wz]T的误差。
