四旋翼姿态解算——互补滤波法补充（融合磁力计）

上一次的博文中（点我打开）介绍了互补滤波法的算法流程和程序实现，但是仅仅只是融合了三轴加速度计和三轴陀螺仪的数据解算出姿态。

所以，我们考虑使用加速度计和磁力计同时对陀螺仪进行校正。这次介绍如何融合磁力计的数据进行姿态解算。
在介绍算法之前，想再提一点自己的感想。很多新手入门姿态解算算法时，最先接触的就是这个Mahony的互补滤波算法，也相对简单一些。有的人觉得数学推导很麻烦，也不愿意花时间研究代码，懵懵懂懂地把程序复制粘贴过去就随便填了几个参数，然后就去看现象。其实大家都知道这种方法是不对的吧，没出问题还好，一旦出了错就不知所措了。很多时候，飞控出了问题，不一定是控制的问题，有时候问题出在姿态解算上（“血与泪”的教训，我当初就是在这个地方被坑了很久）。得到了姿态角后，先看看波形，看看数据怎么样，滤波器有没有用。（不想自己写程序和调试的，请无视这些。）
这里再推荐一个很实用的工具：匿名上位机。功能很强大，可以自己写程序上传数据，在上位机上看波形，一切OK之后再进行下一步的工作。
这是匿名上位机的界面的截图，可以到匿名科创的官网上下载：

好了，开始介绍算法部分吧！
还是按照这张框图得流程来介绍：

首先，假设磁力计测量得到的三轴磁场数据为 。
为方便运算，像此前一样，将数据归一化之后再进行运算。得到载体坐标系b下的标准磁场数据 ，其中 。为表示简便，将归一化之后的数据还是表示为 。
将这组载体坐标系b下测得的数据 旋转到导航坐标系n下，得到 。
坐标系的旋转相当于乘以一个旋转矩阵 。
使用以前推导得到的结论，其为四元数表示的旋转矩阵 ：

得到：

假定理想情况下，让导航坐标系n中x轴指向正北方向，并设这个新的导航坐标系下磁力计数据为 。那么根据高中物理知识分析可以知道，Z轴方向分量（即重力方向分量）相等，水平方向分量相等。 则容易得到：

若取y轴指向正北方向，则：

取x轴或y轴朝向正北，按照自己情况定义。我在程序中取的是x轴，所以后面使用x轴朝正北方向的结果来推导。
将磁力线在导航坐标系n中的理想输出 再次旋转到载体坐标系b中，得到在b系中的理想输出：

将理想输出 和原始输出 作叉积得到误差e(w)，将其作为补偿项送给陀螺仪进行校正：

表示成矩阵形式：

这部分只是用来修正YAW轴的，还要加上加速度的校正补偿项（前面的博文中已经推导过），再送给陀螺仪：

后面的就与前面博文中介绍的步骤一样了，这里不做赘述。

终于可以上代码了：

////////////////////////////////////////////////////////////////////////////////
#define Kp 10.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.008f                          // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.001f                   // half the sample period采样周期的一半

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error
void IMUupdate(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 wx, wy, wz;
float vx, vy, vz;
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;

if(ax*ay*az==0)
return;

if(mx*my*mz == 0)
return;

norm = sqrt(ax*ax + ay*ay + az*az);       //acc数据归一化
ax = ax / norm;
ay = ay / norm;
az = az / norm;

norm = sqrt(mx*mx + my*my + mz*mz);       //mag数据归一化
mx = mx / norm;
my = my / norm;
mz = mz / norm;

//  mx = 0;
//  my = 0;
//  mz = 0;

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 flux (v and w)              估计重力方向和流量/变迁
vx = 2*(q1q3 - q0q2);                                             //四元素中xyz的表示
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);

// error is sum of cross product between reference direction of fields and direction measured by sensors
//  ex = (ay*vz - az*vy) ;                                               //向量外积在相减得到差分就是误差
//  ey = (az*vx - ax*vz) ;
//  ez = (ax*vy - ay*vx) ;

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;

// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;                                              //将误差PI后补偿到陀螺仪，即补偿零点漂移
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;                                          //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减

// integrate quaternion rate and normalise                           //四元素的微分方程
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;

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

Q_ANGLE.z = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
Q_ANGLE.y  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Q_ANGLE.x = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}

照着前面推导的步骤，再跟着程序走一遍，是不是清晰了许多？

推荐一些比较好的资料的链接：
陀螺仪、加速度计 、磁力计的介绍
http://www.360doc.com/content/13/1217/19/235269_337950743.shtml
磁力计工作原理介绍
http://blog.sina.com.cn/s/blog_402c071e0102v8ig.html