笔者最近在做四轴，涉及到地磁计的融合算法，网上大多数是x-IMU的融合代码，但是这段代码对于地磁计的融合说明没有做过多的解释，网上没有相关讨论，仅在阿莫论坛看到一篇相关的代码解释，里面有关于地磁计融合部分的解说，个人觉得说的不是很清楚，虽然是正确的，我这里再补充啰嗦一下。
首先给出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 measurementrecipNorm = invSqrt(ax * ax + ay * ay + az * az);ax *= recipNorm;ay *= recipNorm;az *= recipNorm;// Normalise magnetometer measurementrecipNorm = invSqrt(mx * mx + my * my + mz * mz);mx *= recipNorm;my *= recipNorm;mz *= recipNorm;// Auxiliary variables to avoid repeated arithmeticq0q0 = 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 fieldhx = 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 fieldhalfvx = 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 vectorshalfex = (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 enabledif(twoKi > 0.0f) {integralFBx += twoKi * halfex * (1.0f / sampleFreq);// integral error scaled by KiintegralFBy += twoKi * halfey * (1.0f / sampleFreq);integralFBz += twoKi * halfez * (1.0f / sampleFreq);gx += integralFBx;// apply integral feedbackgy += integralFBy;gz += integralFBz;}else {integralFBx = 0.0f;// prevent integral windupintegralFBy = 0.0f;integralFBz = 0.0f;}// Apply proportional feedbackgx += twoKp * halfex;gy += twoKp * halfey;gz += twoKp * halfez;}// Integrate rate of change of quaterniongx *= (0.5f * (1.0f / sampleFreq));// pre-multiply common factorsgy *= (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 quaternionrecipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);q0 *= recipNorm;q1 *= recipNorm;q2 *= recipNorm;q3 *= recipNorm;}
相信有很多人已经理解了加速度计补偿陀螺仪漂移的原理，这部分代码在x-IMU官网上已经给出，大家可以自行下载()，有能力的可以自行查看Sebastian O.H. Madgwick在2010年4月发表的一篇论文（An efficient orientation filter for inertial and inertial/magneticsensor arrays），可以发现x-IMU官网上的融合代码就是基于此篇论文。
花了一天时间研究地磁融合代码，总算弄明白了其地磁融合的原理。为了让大家理解Madgwick对地磁量的处理方式，我先从加速度计补偿开始说起。
首先，东北天坐标系我们称之为n系（地理坐标系，参考坐标系），载体坐标系我们称之为b系，就是我们飞行器的坐标系。对于四元数法的姿态解算，我们求的就是四元数的值；方向余弦矩阵（用于表示n系和b系的相对关系）中的元素本来应该是三角函数，这里由于我们四元数法，所以矩阵中的元素就成了四元数。所以我们的任务就是求解由四元数构成的方向余弦矩阵nCb（nCb表示从b系到n的转换矩阵，同理，bCn表示从n系到b的转换矩阵，他们的关系是转置）。
显然，上述矩阵是有误差存在的。对于一个确定的向量n，用不同的坐标系表示时，他们所表示的大小和方向一定是相同的。但是由于这两个坐标系的转换矩阵存在误差，那么当一个向量经过这么一个有误差存在的旋转矩阵变换后，在另一个坐标系中肯定和理论值是有偏差的，，我们通过这个偏差来修正这个旋转矩阵。我们刚才说了，这个旋转矩阵的元素是四元数，这就是说我们修正的就是四元数，于是乎我们的姿态就这样被修正了，这才是姿态解算的原理。
我这里再重复一遍，因为这是原理部分。我们的姿态解算求的是四元数，我们是通过修正旋转矩阵中的四元数来达到姿态解算的目的，而不要以为通过加速度计和地磁计来修正姿态，加速度计和地磁计只是测量工具和载体，通过这两个器件表征旋转矩阵的误差存在，然后通过算法修正误差，修正四元数，修正姿态。
自己要先看得起自己，别人才会看得起你
