导：根据我前面的视频又说过这个流程：姿态角来自四元素，四元素取决于陀螺仪，陀螺仪存在温度漂移、零点漂移和时间积分误差，所以使用加计（理想与实际的err值）来pi校准陀螺；
现在，磁力计对航向角yaw的校准，就是同样的道理：使用磁力计（理想与实际的err）来校准陀螺；
故：现在的err构成为：
1.既然磁力计的校准与加速度计的校准具有相似性，同时，此过程就是姿态结算的又一次呈现，朋友们可以先看我的视频总结；再看此文豁然开朗，印象更加深刻；下发连接如下：
https://blog.csdn.net/xiaoxilang/article/details/80326013 博客文章结尾有连接！
2.分析err的如下图：
3.代码分析:请您细看1-9点标注之处，是代码关键点，对照我之前的视频，可相互印证！
void AHRSupdate(unsigned char ahrs_flag)
{
float norm,Spin_rate,P_gain,W_yaw_P[3];
float change_limit,W_P[3];
float q0,q1,q2,q3;
float q0q1,q0q2,q0q3,q1q1,q1q2,q1q3,q2q2,q2q3,q3q3;
float mag_nx,mag_ny,mag_nz,mag_bx,mag_by,true_my,true_mz;
float f_bx,f_by,f_bz;
float err1_x,err1_y,err1_z,err2_x,err2_y,err2_z;
float gx,gy,gz,ax,ay,az,mx,my,mz;
//1.9轴原始数据获得-----------------------------
gx = alldata.ina_gyro1; //right
gy = alldata.ina_gyro2; //front
gz = alldata.ina_gyro3; //up
mx = alldata.ina_mag1; //right
my = alldata.ina_mag2; //front
mz = alldata.ina_mag3; //up
ax = alldata.ina_acc1;//x_acc_filter;//outPut_buf[0][6]; // right
ay = alldata.ina_acc2;//outPut_buf[1][6]; // front
az = alldata.ina_acc3;//outPut_buf[2][6]; // up
//2.计算p参数：gyro_drift_limit = 0.01f*rad,Spin_rate_limit1 = 10.0f*rad,Spin_rate_limit2 = 50.0f*rad
//计算P_gain
Spin_rate = __sqrtf(gx*gx + gy*gy + gz*gz);
if (Spin_rate < Spin_rate_limit1)
P_gain = 1.0f;
if (Spin_rate > Spin_rate_limit2)
P_gain = 5.0f;
if ((Spin_rate >= Spin_rate_limit1)&&(Spin_rate <= Spin_rate_limit2))
P_gain = Spin_rate/(Spin_rate_limit1);
//3.因为陀螺需要加计个磁力计校准：所以先归一化加计和磁力计三轴数据
// normalise the measurements
norm = __sqrtf(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = __sqrtf(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;
q0 = ahrs_q[0];
q1 = ahrs_q[1];
q2 = ahrs_q[2];
q3 = ahrs_q[3];
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;
//4.准备数据以求磁力计的实际和理想
mag_nx = 2.0f*mx*(0.5f - q2q2 - q3q3) + 2.0f*my*(q1q2 - q0q3) + 2.0f*mz*(q1q3 + q0q2);
mag_ny = 2.f*mx*(q1q2 + q0q3) + 2.0f*my*(0.5f - q1q1 - q3q3) + 2.0f*mz*(q2q3 - q0q1);
mag_nz = 2.0f*mx*(q1q3 - q0q2) + 2.0f*my*(q2q3 + q0q1) + 2.0f*mz*(0.5f - q1q1 - q2q2);
true_my = __sqrtf(mag_nx*mag_nx+mag_ny*mag_ny);
true_mz = mag_nz;
mag_bx = 2.0f*true_my*(q1q2+q0q3)+2.0f*true_mz*(q1q3-q0q2);
mag_by = 2.0f*true_my*(0.5f-q1q1-q3q3)+2.0f*true_mz*(q2q3+q0q1);
f_bx = -2.0f*(q1q3-q0q2);
f_by = -2.0f*(q2q3+q0q1);
f_bz = -2.0f*(0.5f-q1q1-q2q2);
//5.加计误差：利用叉乘计算重力加速度参考向量经过Tbn矩阵转换到载体系后和实测的载体系加速度之间的误差
err1_x = ay*f_bz - az*f_by;
err1_y = az*f_bx - ax*f_bz;
err1_z = ax*f_by - ay*f_bx;
err2_x = 0;//6.磁力计误差：利用差乘...注意：只校航向！！！
err2_y = 0;
err2_z = mx*mag_by - my*mag_bx;
W_yaw_P[0] = err2_x*P_gain*Kp_yaw;//7.磁力计的pi校准
W_yaw_P[1] = err2_y*P_gain*Kp_yaw;
W_yaw_P[2] = err2_z*P_gain*Kp_yaw;
if (Spin_rate<Spin_rate_limit1)
{
W_sum_I[0] = W_sum_I[0]+err2_x*Ki_yaw+err1_x*Ki_acc;
W_sum_I[1] = W_sum_I[1]+err2_y*Ki_yaw+err1_y*Ki_acc;
W_sum_I[2] = W_sum_I[2]+err2_z*Ki_yaw+err1_z*Ki_acc;
W_sum_I_time++;
}
if (W_sum_I_time>=50)
{
change_limit = gyro_drift_limit*W_sum_I_time*T;
W_sum_I[0] = Math_fConstrain(W_sum_I[0],-change_limit,change_limit);
W_sum_I[1] = Math_fConstrain(W_sum_I[1],-change_limit,change_limit);
W_sum_I[2] = Math_fConstrain(W_sum_I[2],-change_limit,change_limit);
W_I[0] = W_I[0]+W_sum_I[0];
W_I[1] = W_I[1]+W_sum_I[1];
W_I[2] = W_I[2]+W_sum_I[2];
W_sum_I[0] = 0;
W_sum_I[1] = 0;
W_sum_I[2] = 0;
W_sum_I_time = 0;
}
W_P[0] = err1_x*P_gain*Kp_acc;//8.加计的pi校准
W_P[1] = err1_y*P_gain*Kp_acc;
W_P[2] = err1_z*P_gain*Kp_acc;
gx = gx+W_I[0]+W_P[0]+W_yaw_P[0];//9.陀螺的最终校准结果
gy = gy+W_I[1]+W_P[1]+W_yaw_P[1];
gz = gz+W_I[2]+W_P[2]+W_yaw_P[2];
ahrs_qLonge_Kuta(gx, gy, gz);//10.由陀螺计算四元素
norm_ahrsq(); //11.四元素归一化,为下一次过程准备数据！！！ //12.由四元素到姿态角 ，请见视频说明！！！！
}
4.参考技术文档
陀螺仪、加速度计 、磁力计的介绍
http://www.360doc.com/content/13/1217/19/235269_337950743.shtml
磁力计工作原理介绍
http://blog.sina.com.cn/s/blog_402c071e0102v8ig.html
四旋翼姿态解算——互补滤波法补充（融合磁力计）
https://blog.csdn.net/hongbin_xu/article/details/59110226