姿态解算（b表示机体坐标系 c天南地北坐标系）
文章内容来自：https://www.bilibili.com/video/av13035245/?spm_id_from=333.338.recommend_report.5
用做以后复习使用
下面记录一下在观看的时候所遇到的几个误区
1.在融合加速度计的时候对重力方向向量乘了一个方向余弦矩阵进行变换，注意此时是坐标系发生了变化，而不是向量发生了变化，向量的方向和大小始终不变，变得只是表达方式。例如原来重力向量指向地面，经过余弦矩阵之后仍是指向地面。
我们可以通过四元数计算出来重力加速度，但是，显示这个加速度和正确的重力加速度之间存在误差。也就是说我们把四元数转化成重力加速度和加速计测量到的准确的重力加速度进行对比,给四元数算出来的值加了一个标尺从而进行修正。
关于右手坐标系的说明
1让x轴从掌心穿过 2 四指握向y轴 3拇指所指方向就是z轴。
文章最后是匿名的姿态解算程序，磁力计的融合不是特别困难，磁力计的校准比较困难。
加速度计校准方法：
1、调头校准法
想必大家应该知道，就是 X 朝上朝下、 Y 朝上朝下、 Z 朝上朝下，共 6 个方向，分别找
到 X、 Y、 Z 三个轴的最大最小值，从中求得加速度计的零偏值和灵敏度。调头校准法非常简
单、实用。但是实施起来也是有条件的，那就是 XYZ 朝上朝下时，要尽可能的与重力方向保
持平行，越准越好，否则会影响校准精度。
2、拟合校准法
拟合校准法一般是通过离线采集到的一系列加速度数据， 然后通过类似本文所重点介绍
的 AntAcc 这样的软件来获得精准的加速度计校准参数。
实验发现，采用 AntAcc 这样的加速度计离线校准方法， 1）可以获得更高的精准度， 2）
可以不依赖于水平台面， 3）可以不用去找准 XYZ 朝上朝下的具体位置。
用户只需要手持（或者其他夹持方式）着自己的飞行控制器（或者加速度传感器）在空
中朝各个方向慢速旋转几次就可以了。
2
相关软件的下载地址： http://blog.sina.com.cn/s/blog_402c071e0102wnm4.html
代码的校准还没看懂。。。。。。
void IMUupdate(float half_T,float gx, float gy, float gz, float ax, float ay, float az,float *rol,float *pit,float *yaw)
{
float ref_err_lpf_hz;
static float yaw_correct;
float mag_norm_tmp;
static xyz_f_t mag_tmp;
static float yaw_mag;
mag_norm_tmp = 20 *(6.28f *half_T);
mag_norm_xyz = my_sqrt(ak8975.Mag_Val.x * ak8975.Mag_Val.x + ak8975.Mag_Val.y * ak8975.Mag_Val.y + ak8975.Mag_Val.z * ak8975.Mag_Val.z);
if( mag_norm_xyz != 0)
{
mag_tmp.x += mag_norm_tmp *( (float)ak8975.Mag_Val.x /( mag_norm_xyz ) - mag_tmp.x);
mag_tmp.y += mag_norm_tmp *( (float)ak8975.Mag_Val.y /( mag_norm_xyz ) - mag_tmp.y);
mag_tmp.z += mag_norm_tmp *( (float)ak8975.Mag_Val.z /( mag_norm_xyz ) - mag_tmp.z);
}
/*
void simple_3d_trans(_xyz_f_t *ref, _xyz_f_t *in, _xyz_f_t *out)
罗盘数据是机体坐标下的，且磁场方向不是平行于地面，如果飞机倾斜，投影计算的角度会存在误差。
此函数可在一定范围内做近似转换，让结果逼近实际角度，减小飞机倾斜的影响。
注意：该函数内的计算并不是正确也不是准确的，正确的计算相对复杂，这里不给出，在未来的版本中会再更新。
*/
simple_3d_trans(&reference_v,&mag_tmp,&mag_sim_3d);
mag_norm = my_sqrt(mag_sim_3d.x * mag_sim_3d.x + mag_sim_3d.y *mag_sim_3d.y);
if( mag_sim_3d.x != 0 && mag_sim_3d.y != 0 && mag_sim_3d.z != 0 && mag_norm != 0)
{
yaw_mag = fast_atan2( ( mag_sim_3d.y/mag_norm ) , ( mag_sim_3d.x/mag_norm) ) *57.3f;
}
//=============================================================================
// 计算等效重力向量 理论上的加速度 问题：方向是指向哪里的怎么和实际加速度方向融合
reference_v.x = 2*(ref_q[1]*ref_q[3] - ref_q[0]*ref_q[2]);
reference_v.y = 2*(ref_q[0]*ref_q[1] + ref_q[2]*ref_q[3]);
reference_v.z = 1 - 2*(ref_q[1]*ref_q[1] + ref_q[2]*ref_q[2]);//ref_q[0]*ref_q[0] - ref_q[1]*ref_q[1] - ref_q[2]*ref_q[2] + ref_q[3]*ref_q[3];
//这是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。
//根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。
//所以这里的vx\y\z，其实就是当前的欧拉角（即四元数）的机体坐标参照系上，换算出来的重力单位向量。
//=============================================================================
if(acc_ng_cali)
{
if(acc_ng_cali==2)
{
acc_ng_offset.x = 0;
acc_ng_offset.y = 0;
acc_ng_offset.z = 0;
}
acc_ng_offset.x += 10 *TO_M_S2 *(ax - 4096*reference_v.x) *0.0125f ;
acc_ng_offset.y += 10 *TO_M_S2 *(ay - 4096*reference_v.y) *0.0125f ;
acc_ng_offset.z += 10 *TO_M_S2 *(az - 4096*reference_v.z) *0.0125f ;
acc_ng_cali ++;
if(acc_ng_cali>=82) //start on 2
{
acc_ng_cali = 0;
}
}
acc_ng.x = 10 *TO_M_S2 *(ax - 4096*reference_v.x) - acc_ng_offset.x;
acc_ng.y = 10 *TO_M_S2 *(ay - 4096*reference_v.y) - acc_ng_offset.y;
acc_ng.z = 10 *TO_M_S2 *(az - 4096*reference_v.z) - acc_ng_offset.z;
acc_3d_hg.z = acc_ng.x *reference_v.x + acc_ng.y *reference_v.y + acc_ng.z *reference_v.z;
// 计算加速度向量的模
norm_acc = my_sqrt(ax*ax + ay*ay + az*az);
//实际加速度
if(ABS(ax)<4400 && ABS(ay)<4400 && ABS(az)<4400 )
{
//把加计的三维向量转成单位向量。
ax = ax / norm_acc;//4096.0f;
ay = ay / norm_acc;//4096.0f;
az = az / norm_acc;//4096.0f;
if( 3800 < norm_acc && norm_acc < 4400 )
{
/* 叉乘得到误差 */
ref.err_tmp.x = ay*reference_v.z - az*reference_v.y;
ref.err_tmp.y = az*reference_v.x - ax*reference_v.z;
//ref.err_tmp.z = ax*reference_v.y - ay*reference_v.x;
/* 误差低通 */
ref_err_lpf_hz = REF_ERR_LPF_HZ *(6.28f *half_T);
ref.err_lpf.x += ref_err_lpf_hz *( ref.err_tmp.x - ref.err_lpf.x );
ref.err_lpf.y += ref_err_lpf_hz *( ref.err_tmp.y - ref.err_lpf.y );
// ref.err_lpf.z += ref_err_lpf_hz *( ref.err_tmp.z - ref.err_lpf.z );
ref.err.x = ref.err_lpf.x;//
ref.err.y = ref.err_lpf.y;//
// ref.err.z = ref.err_lpf.z ;
}
}
else
{
ref.err.x = 0;
ref.err.y = 0 ;
// ref.err.z = 0 ;
}
/* 误差积分 */
ref.err_Int.x += ref.err.x *Ki *2 *half_T ;
ref.err_Int.y += ref.err.y *Ki *2 *half_T ;
ref.err_Int.z += ref.err.z *Ki *2 *half_T ;
/* 积分限幅 */
ref.err_Int.x = LIMIT(ref.err_Int.x, - IMU_INTEGRAL_LIM ,IMU_INTEGRAL_LIM );
ref.err_Int.y = LIMIT(ref.err_Int.y, - IMU_INTEGRAL_LIM ,IMU_INTEGRAL_LIM );
ref.err_Int.z = LIMIT(ref.err_Int.z, - IMU_INTEGRAL_LIM ,IMU_INTEGRAL_LIM );
if( reference_v.z > 0.0f )
{
if( fly_ready )
{
yaw_correct = Kp *0.2f *To_180_degrees(yaw_mag - Yaw);
//已经解锁，只需要低速纠正。
}
else
{
yaw_correct = Kp *1.5f *To_180_degrees(yaw_mag - Yaw);
//没有解锁，视作开机时刻，快速纠正
}
}
else
{
yaw_correct = 0; //角度过大，停止修正，修正的目标值可能不正确
}
ref.g.x = (gx - reference_v.x *yaw_correct) *ANGLE_TO_RADIAN + ( Kp*(ref.err.x + ref.err_Int.x) ) ; //IN RADIAN
ref.g.y = (gy - reference_v.y *yaw_correct) *ANGLE_TO_RADIAN + ( Kp*(ref.err.y + ref.err_Int.y) ) ; //IN RADIAN
ref.g.z = (gz - reference_v.z *yaw_correct) *ANGLE_TO_RADIAN;//这是什么
/* 用叉积误差来做PI修正陀螺零偏 */
// integrate quaternion rate and normalise 一阶龙哥库塔法计算四元数
ref_q[0] = ref_q[0] +(-ref_q[1]*ref.g.x - ref_q[2]*ref.g.y - ref_q[3]*ref.g.z)*half_T;
ref_q[1] = ref_q[1] + (ref_q[0]*ref.g.x + ref_q[2]*ref.g.z - ref_q[3]*ref.g.y)*half_T;
ref_q[2] = ref_q[2] + (ref_q[0]*ref.g.y - ref_q[1]*ref.g.z + ref_q[3]*ref.g.x)*half_T;
ref_q[3] = ref_q[3] + (ref_q[0]*ref.g.z + ref_q[1]*ref.g.y - ref_q[2]*ref.g.x)*half_T;
/* 四元数规一化 normalise quaternion */
norm_q = my_sqrt(ref_q[0]*ref_q[0] + ref_q[1]*ref_q[1] + ref_q[2]*ref_q[2] + ref_q[3]*ref_q[3]);
ref_q[0] = ref_q[0] / norm_q;
ref_q[1] = ref_q[1] / norm_q;
ref_q[2] = ref_q[2] / norm_q;
ref_q[3] = ref_q[3] / norm_q;
*rol = fast_atan2(2*(ref_q[0]*ref_q[1] + ref_q[2]*ref_q[3]),1 - 2*(ref_q[1]*ref_q[1] + ref_q[2]*ref_q[2])) *57.3f;
*pit = asin(2*(ref_q[1]*ref_q[3] - ref_q[0]*ref_q[2])) *57.3f;
*yaw = fast_atan2(2*(-ref_q[1]*ref_q[2] - ref_q[0]*ref_q[3]), 2*(ref_q[0]*ref_q[0] + ref_q[1]*ref_q[1]) - 1) *57.3f ;//
}