Pixhawk之姿态解算篇（3）_源码姿态解算算法分析

一、开篇

        终于到ardupilot源代码的姿态解算了，有了前期关于mahony姿态解算算法的基础以后，理解源代码的姿态解算算法就快多了，所有的东西都在脑海中初步有了一个框架；首先要做什么，然后再做什么，再然后捏~~~反正容易上手的。

        2016.04.04日晚，别人都在嗨，而我却在实验室苦逼的工作着，今晚最大的收获就是发现了“新大陆”-----“北航可靠飞行控制研究组”，其喜悦之情绝不亚于哥伦布发现新大陆。他们才是专业的啊，看看他们毕业生的去向，不是研究所就是出国深造，好吧，人家才是专业搞科研的，不食人间烟火，那么问题来了：DJI的员工都是在哪招来的呢？！哎，我是俗人一个，经济基础决定上层建筑，好好学习才能挣大钱。最近他们开设了一门课程《多旋翼飞行器设计与控制》，课程体系安排的非常好，现在更新到第四讲了（听北航一个博士说只有PPT没有视频，感谢Mallin的帮助，成功打入内部），PPT也足够了，相当上档次啊，课程到2016.06.30结束，正好可以把无人机的整个架构理解完，找工作去~~~

        在下面的基础知识部分先分享一部分“北航”的研究成果，特别是气动方面的，以前一点概念都没有，只看着超跑的流线型非常炫酷，不知其原因，特此记录，大家共勉。

二、版权声明

博主：summer

声明：喝水不忘挖井人，转载请注明出处。

原文地址：http://blog.csdn.net/qq_21842557

联系方式：[email protected]

技术交流QQ：1073811738

三、实验平台

Software Version：PX4Firmware

Hardware Version：pixhawk

IDE：eclipse Juno （Windows）

四、基础知识（均来自北航可靠飞行控制研究组）

1、无人机飞行的气动模型与分析

        1）多旋翼前飞情形：在下图中，因为螺旋桨的柔性，诱导的来流会产生阻力。

        如果多旋翼重心在桨盘平面下方，那么阻力形成的力矩会促使多旋翼俯仰角转向0度方向。

        如果多旋翼重心在桨盘平面上方，那么阻力形成的力矩会促使多旋翼俯仰角朝发散方向发展，直至翻转。因此，当多旋翼前飞时，重心在桨盘平面的下方会使前飞运动稳定。

        2）多旋翼风干扰情形：在下图中，当阵风吹来，因为螺旋桨的柔性，诱导的来流会在产生阻力。

        如果多旋翼重心在下，那么阻力形成的力矩会促使多旋翼俯仰角朝发散的方向发展，直至翻转。

        如果多旋翼重心在上，那么阻力形成的力矩会促使多旋翼俯仰超0度方向发展。因此，当多旋翼受到外界风干扰时，重心在桨盘平面的上方可以抑制扰动。

        3）综上所述：无论重心在桨盘平面的上方或下方都不能使多旋翼稳定。因此需要通过反馈控制将多旋翼平衡。然而，如果重心在桨盘平面很靠上的位置，会使多旋翼某个运动模态很不稳定。因此，实际中建议将重心配置在飞行器桨盘周围，可以稍微靠下。这样控制器控制起来更容易些。

2、气动布局

        对外形进行设计主要是为了降低飞行时的阻力。按其产生的原因不同可分为

（1）摩擦阻力

（2）压差阻力

（3）诱导阻力

（4）干扰阻力

        要减少这些阻力，需要妥善考虑和安排各部件之间的相对位置关系，部件连接处尽量圆滑过渡，减少漩涡产生。

        因此它与物体的迎风面积有很大关系，迎风面积越大，压差阻力也越大。物体的形状也对压差阻力影响很大。如上图所示的三个物体，圆盘的压差阻力最大，球体次之，而流线体的最小，就压差阻力而言可以是平板压差阻力的1/20。

        设计建议：（法拉利、保驰捷等超跑的流线型车就是很好的榜样，宝马 Z4也可以，奔驰Smart就太low了，差点忘记特斯拉了）

（1）需要考虑多旋翼前飞时的倾角，减少最大迎风面积。

（2）并设计流线型机身。

（3）考虑和安排各部件之间的相对位置关系，部件连接处尽量圆滑过渡，飞机表面也要尽量光滑。

（4）通过CFD仿真（Computational Fluid Dynamics：计算流体动力学）计算阻力系数，不断优化。

        昨天见到了零度的四旋翼，外形设计的就是好，可惜只靠外形还是干不过DJI的，加油吧，零度。

五、正文（源代码的姿态解算算法）

1、进程入口：voidAttitudeEstimatorQ::task_main()

        1）订阅所需要的topics，注意sensor_combined，传感器数据都是靠它来的。

        在订阅时使用ORB_ID(sensor_combined)获取ID号，该ID号代表了从topic_name到topicmetadata structure name之间的转换桥梁。在task_main()的初始部分使用uORB模型的orb_subscribe()函数获取在sensors.cpp中通过orb_advertise()函数广播的传感器信息（sensor_combined）。由此说明在使用之前需要通过orb_advertise()函数之后才能在需要其数据的地方使用orb_subscribe()获取。如果没有使用，订阅者可以订阅，但是接收不到有效数据。

        关于uOBR模型的不再赘述，详细介绍参看：http://blog.csdn.net/freeape/article/details/46880637

和http://www.pixhawk.com/start?id=zh%2Fdev%2Fshared_object_communication&go

        px4_poll(fds,1, 1000)：配置阻塞时间，1ms读取一次sensor_combined的数据。

        必须有了orb_advertise()函数和orb_subscribe()函数（通过它获取orb_copy()函数中的参数handle）之后才可以使用orb_copy(ORB_ID(sensor_combined),_sensors_sub, &sensors)函数获取sensors的实际有效数据。

        首先是读取gyro的数据：

 float gyro[3];
 for (unsigned j = 0; j < 3; j++)
 {
     if (sensors.gyro_integral_dt[i] > 0)
     {
         gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6);
 }
  else{
         /* fall back to angular rate */
         gyro[j] = sensors.gyro_rad_s[i * 3 + j];
         }
 }

        然后以同样的方法读取accel和mag的数据。

 Void DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority)
 {
         DataValidator *next = _first;
         unsigned i = 0;
         while (next != nullptr) {
             if (i == index) {
                 next->put(timestamp, val, error_count, priority);//goto
                 break;
                 }
             next = next->sibling();//下一组数据
             i++;
             }
 }

        最终还是goto到put函数。

 Void DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in)
 {
     _event_count++;
     if (error_count_in > _error_count) {
         _error_density += (error_count_in - _error_count);
     } else if (_error_density > 0) {
         _error_density--;
     }
     _error_count = error_count_in;
     _priority = priority_in;
     for (unsigned i = 0; i < _dimensions; i++) {//_dimensions=3
         if (_time_last == 0) {//时间变量已经初始化为0
             _mean[i] = 0;
             _lp[i] = val[i];
             _M2[i] = 0;
         } else {
             float lp_val = val[i] - _lp[i];
             float delta_val = lp_val - _mean[i];
             _mean[i] += delta_val / _event_count;
             _M2[i] += delta_val * (lp_val - _mean[i]);
             _rms[i] = sqrtf(_M2[i] / (_event_count - 1));
             if (fabsf(_value[i] - val[i]) < 0.000001f) {
                 _value_equal_count++;
             } else {
                 _value_equal_count = 0;
             }
         }
         // XXX replace with better filter, make it auto-tune to update rate
         _lp[i] = _lp[i] * 0.5f + val[i] * 0.5f;
         _value[i] = val[i];
     }
     _time_last = timestamp;
 }

        详细介绍使用方法：主要是将三类参数分别建立相应的 DataValidatorGroup类来对数据进行处理。

        DataValidatorGroup类： _voter_gyro、_voter_accel、_voter_mag

         调用方法：

         1）首先gyro、accel、mag每次读取数据都是三组三组的读取

         2）先将每组的数据（例如gyro将三个维度的的传感器数据put入（如_voter_gyro.put(...))）DataValidatorGroup中，并goto到DataValidator::put函数

         3）在DataValidator函数中计算数据的误差、平均值、并进行滤波。

    滤波入口的put函数：

         val=传感器读取的数据

         _lp=滤波器的系数（lowpass value）

         初始化：由上图可知当第一次读到传感器数据时_mean和_M2置0，_lp=val；

         lp_val= val - _lp

         delta_val= lp_val - _mean

         _mean= (平均值）每次数据读取时，每次val和_lp的差值之和的平均值 _mean[i] += delta_val / _event_count

         _M2= （均方根值）delta_val * (lp_val - _mean)的和

         _rms= 均方根值sqrtf(_M2[i] / (_event_count - 1))

         优化滤波器系数：_lp[i]= _lp[i] * 0.5f + val[i] * 0.5f

         _value= val ：get_best()函数的最后调用该结果(通过比较三组数据的confidence大小决定是否选取）。

    滤波器的confidence函数（信任度函数，貌似模糊控制理论有个隶属函数，应该类似的功能）：返回值是对上N次测量的验证的信任程度，其值在0到1之间，越大越好。返回值是返回上N次测量的误差诊断，用于get_best函数选择最优值，选择的方法如下：

 Switch if:

         1)the confidence is higher and priority is equal or higher

         2)the confidence is no less than 1% different and the priority is higher

2、根据_voter_gyro、_voter_accel、_voter_mag三个参数的failover_count函数判断是否存在数据获取失误问题，并通过mavlink协议显示错误原因。

3、根据_voter_gyro、_voter_accel、_voter_mag三个参数的get_vibration_factor函数判断是否有震动现象，返回值是float型的RSM值，其代表振动的幅度大小。

 Float DataValidatorGroup::get_vibration_factor(uint64_t timestamp)
 {
     DataValidator *next = _first;
     float vibe = 0.0f;
     /* find the best RMS value of a non-timed out sensor */
     while (next != nullptr) {
         if (next->confidence(timestamp) > 0.5f) {
             float* rms = next->rms();
             for (unsigned j = 0; j < 3; j++) {
                 if (rms[j] > vibe) {
                     vibe = rms[j];//获取最大值
                 }
             }
         }
         next = next->sibling();
     }
     return vibe;//返回DataValidatorGroup中的三组中的最大的振动值
 }

        将rms变量（原来put函数中的_rms）传回主函数中和_vibration_warning_threshold进行判断。

4、通过uORB模型获取vision和mocap的数据（视觉和mocap）

 // Update vision and motion capture heading
         bool vision_updated = false;
         orb_check(_vision_sub, &vision_updated);
         bool mocap_updated = false;
         orb_check(_mocap_sub, &mocap_updated);
         if (vision_updated) {
             orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision);
             math::Quaternion q(_vision.q);
             math::Matrix<3, 3> Rvis = q.to_dcm();
             math::Vector<3> v(1.0f, 0.0f, 0.4f);
             // Rvis is Rwr (robot respect to world) while v is respect to world.
             // Hence Rvis must be transposed having (Rwr)' * Vw
             // Rrw * Vw = vn. This way we have consistency
             _vision_hdg = Rvis.transposed() * v;
         }
         if (mocap_updated) {
             orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
             math::Quaternion q(_mocap.q);
             math::Matrix<3, 3> Rmoc = q.to_dcm();
             math::Vector<3> v(1.0f, 0.0f, 0.4f);
             // Rmoc is Rwr (robot respect to world) while v is respect to world.
             // Hence Rmoc must be transposed having (Rwr)' * Vw
             // Rrw * Vw = vn. This way we have consistency
             _mocap_hdg = Rmoc.transposed() * v;
         }

5、位置加速度获取（position，注意最后在修正时会用到该处的_pos_acc）（484）

        首先检测是否配置了自动磁偏角获取，如果配置了则用当前的经纬度（longitude and latitude）通过get_mag_declination(_gpos.lat,_gpos.lon)函数获取当前位置的磁偏角（magnetic declination），实现过程就是一系列的算算算，自己跟踪源码看吧，用地面站校准罗盘的应该比较熟悉这个。然后就是获取机体的速度，通过速度计算机体的加速度。

 bool gpos_updated;
         orb_check(_global_pos_sub, &gpos_updated);
         if (gpos_updated) {
             orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);
         if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) {
                 /* set magnetic declination automatically */
                 _mag_decl = math::radians(get_mag_declination(_gpos.lat, _gpos.lon));
             }
         }
         if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f && _inited) {
             /* position data is actual */
             if (gpos_updated) {
                 Vector<3> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);
                 /* velocity updated */
                 if (_vel_prev_t != 0 && _gpos.timestamp != _vel_prev_t) {
                     float vel_dt = (_gpos.timestamp - _vel_prev_t) / 1000000.0f;
                     /* calculate acceleration in body frame ：速度之差除时间*/
                     _pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
                 }
                 _vel_prev_t = _gpos.timestamp;
                 _vel_prev = vel;
             }
         } else {
             /* position data is outdated, reset acceleration */
             _pos_acc.zero();
             _vel_prev.zero();
             _vel_prev_t = 0;
         }

6、update函数（528行）

        接下来就是528行的updata函数了，非常重要，这个函数主要用于对四元数向量_q进行初始化赋值或者更新。

        --------------------------------------------------------------------------------------------------------------------------

        首先判断是否是第一次进入该函数，第一次进入该函数先进入init函数初始化，源码如下。

 bool AttitudeEstimatorQ::init()
 {
     // Rotation matrix can be easily constructed from acceleration and mag field vectors
     // 'k' is Earth Z axis (Down) unit vector in body frame
     Vector<3> k = -_accel;
     k.normalize();
     // 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
     Vector<3> i = (_mag - k * (_mag * k));
     i.normalize();
     // 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
     Vector<3> j = k % i;
     // Fill rotation matrix
     Matrix<3, 3> R;
     R.set_row(0, i);
     R.set_row(1, j);
     R.set_row(2, k);
     // Convert to quaternion
     _q.from_dcm(R);
     _q.normalize();
     if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
         PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
         _q.length() > 0.95f && _q.length() < 1.05f) {
         _inited = true;
     } else {
         _inited = false;
     }
     return _inited;
 }

        初始化方法：

        k= -_accel 然后归一化k，k为加速度传感器测量到加速度方向向量，由于第一次测量数据时无人机一般为平稳状态无运动状态，所以可以直接将测到的加速度视为重力加速度g，以此作为dcm旋转矩阵的第三行k（这个介绍过了）。

        i= （_mag - k * (_mag * k)) _mag向量指向正北方，k*(_mag*k) 正交correction值，对于最终的四元数归一化以后的范数可以在正负5%以内；感觉不如《DCM IMU:Theory》中提出的理论“强制正交化”修正的好，Renormalization算法在ardupilot的上层应用AP_AHRS_DCM中使用到了。

        j= k%i ：外积、叉乘。关于上面的Vector<3>k = -_accel，Vector<3>相当于一个类型（int）定义一个变量k，然后把-_accel负值给k，在定义_accel时也是使用Vector<3>，属于同一种类型的，主要就是为了考虑其实例化过程（类似函数重载）。

        然后以模板的形式使用“Matrix<3, 3> R”建立3x3矩阵R，通过set_row()函数赋值。

 /**
  * set row from vector
  */
 void set_row(unsigned int row, const Vector v) {
     for (unsigned i = 0; i < N; i++) {
         data[row][i] = v.data[i];
     }
 }

        第一行赋值i 向量指向北，第二行赋值j 向量指向东，第三行赋值k向量指向DOWN。然后就是把DCM转换为四元数_q （使用from_dcm），并归一化四元数，一定要保持归一化的思想。来一个比较牛掰的求范数的倒数的快速算法（mahony的算法实现用到的）：

 float invSqrt(float x) {
     float halfx = 0.5f * x;
     float y = x;
     long i = *(long*)&y;
     i = 0x5f3759df - (i>>1);
     y = *(float*)&i;
     y = y * (1.5f - (halfx * y * y));
     return y;
 }

        具体为什么这么实现的还是看wiki连接吧：https://en.wikipedia.org/wiki/Fast_inverse_square_root

        其中DCM转四元数的算法如下，tr>0时比较好理解，别的情况被简写的看不透了。后续给出原始代码便于理解。

 tr = dcm.data[0][0] + dcm.data[1][1] + dcm.data[2][2]
     如果 tr>0
         s=(tr+1)^0.5  _q[0]= 0.5*s
         _q[1]=(dcm(3,2)-dcm(2,3))*s
         _q[2]=(dcm(1,3)-dcm(3,1))*s
         _q[3]=(dcm(2,1)-dcm(1,2))*s

        其他情况去看代码吧，不好解释。然后_q 单位化结束初始化。PS：另外根据DCM求取四元数的算法是参考MartinJohn Baker的，就是上述的原始版，该算法在AP_Math/quaternion.cpp中，链接：

http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm

        源码如下。

 void Quaternion::from_rotation_matrix(const Matrix3f &m)
 {
     const float &m00 = m.a.x;
     const float &m11 = m.b.y;
     const float &m22 = m.c.z;
     const float &m10 = m.b.x;
     const float &m01 = m.a.y;
     const float &m20 = m.c.x;
     const float &m02 = m.a.z;
     const float &m21 = m.c.y;
     const float &m12 = m.b.z;
     float &qw = q1;
     float &qx = q2;
     float &qy = q3;
     float &qz = q4;
     float tr = m00 + m11 + m22;
     if (tr > 0) {
         float S = sqrtf(tr+1) * 2;
         qw = 0.25f * S;
         qx = (m21 - m12) / S;
         qy = (m02 - m20) / S;
         qz = (m10 - m01) / S;
     } else if ((m00 > m11) && (m00 > m22)) {
         float S = sqrtf(1.0f + m00 - m11 - m22) * 2;
         qw = (m21 - m12) / S;
         qx = 0.25f * S;
         qy = (m01 + m10) / S;
         qz = (m02 + m20) / S;
     } else if (m11 > m22) {
         float S = sqrtf(1.0f + m11 - m00 - m22) * 2;
         qw = (m02 - m20) / S;
         qx = (m01 + m10) / S;
         qy = 0.25f * S;
         qz = (m12 + m21) / S;
     } else {
         float S = sqrtf(1.0f + m22 - m00 - m11) * 2;
         qw = (m10 - m01) / S;
         qx = (m02 + m20) / S;
         qy = (m12 + m21) / S;
         qz = 0.25f * S;
     }
 }

        ---------------------------------------------------------------------------------------------------------------------------------          

        如果不是第一次进入该函数，则判断是使用什么mode做修正的，比如vision、mocap、acc和mag（DJI精灵4用的视觉壁障应该就是这个vision），Hdg就是heading。

 if (_ext_hdg_mode > 0 && _ext_hdg_good) {
         if (_ext_hdg_mode == 1) {
             // Vision heading correction
             // Project heading to global frame and extract XY component
             Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg);
             float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
             // Project correction to body frame
             corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg;
         }
         if (_ext_hdg_mode == 2) {
             // Mocap heading correction
             // Project heading to global frame and extract XY component
             Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg);
             float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
             // Project correction to body frame
             corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg;
         }
     }

        _ext_hdg_mode== 1、2时都是利用vision数据和mocap数据对gyro数据进行修正，下面的global frame就是所谓的earthframe。

         ---------------------------------------------------------------------------------------------------------------------------------

        _ext_hdg_mode== 0利用磁力计修正。

 if (_ext_hdg_mode == 0  || !_ext_hdg_good) {
         // Magnetometer correction
         // Project mag field vector to global frame and extract XY component
         Vector<3> mag_earth = _q.conjugate(_mag); //b系到n系
         float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
         // Project magnetometer correction to body frame
         corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag; //n系到b系
     }

        _w_mag为mag的权重。PS：发现一个规律，在不太理解C++的情况下看代码的算法过程中经常性的不知道某行代码是做什么的，在哪定义的，比如这个Vector<3>，前面已经介绍过它了，只要有它这样的做前缀的，后面的变量就是类似int定义一个变量一样，几乎都在PX4Firmware/src/lib/mathlib/math/Quaternion.hpp中，进行实例展开的。磁力计数据为_mag。mag_earth= _q.conjugate(_mag)，这行代码的含义为先将归一化的_q的旋转矩阵R（b系转n系）乘以_mag向量（以自身机体坐标系为视角看向北方的向量表示），得到n系（NED坐标系）下的磁力计向量。如下就是conjugate函数，其过程就是实现从b系到n系的转换，使用左乘。

 /**
      * conjugation//b系到n系
      */
     Vector<3> conjugate(const Vector<3> &v) const {
         float q0q0 = data[0] * data[0];
         float q1q1 = data[1] * data[1];
         float q2q2 = data[2] * data[2];
         float q3q3 = data[3] * data[3];
         return Vector<3>(
                 v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) +
                 v.data[1] * 2.0f * (data[1] * data[2] - data[0] * data[3]) +
                 v.data[2] * 2.0f * (data[0] * data[2] + data[1] * data[3]),

                 v.data[0] * 2.0f * (data[1] * data[2] + data[0] * data[3]) +
                 v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) +
                 v.data[2] * 2.0f * (data[2] * data[3] - data[0] * data[1]),

                 v.data[0] * 2.0f * (data[1] * data[3] - data[0] * data[2]) +
                 v.data[1] * 2.0f * (data[0] * data[1] + data[2] * data[3]) +
                 v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3)
         );
     }
     mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);

        只考虑Vector<3> mag_earth中的前两维的数据mag_earth(1)和mag_earth(0)（即x、y，忽略z轴上的偏移），通过arctan（mag_earth(1),mag_earth(0)）得到的角度和前面根据经纬度获取的磁偏角做差值得到纠偏误差角度mag_err ，_wrap_pi函数是用于限定结果-pi到pi的函数，源码如下。

 __EXPORT float _wrap_pi(float bearing)
 {
     /* value is inf or NaN */
     if (!isfinite(bearing)) {
         return bearing;
     }
     int c = 0;
 //大于pi则与2pi做差取补
     while (bearing >= M_PI_F) {//M_PI_F==3.14159265358979323846f
         bearing -= M_TWOPI_F;//M_TWOPI_F==2*M_PI_F
         if (c++ > 3) {
             return NAN;//NaN==not a number
         }
     }
     c = 0;
 //小于-pi则与2pi做和取补
     while (bearing < -M_PI_F) {
         bearing += M_TWOPI_F;
         if (c++ > 3) {
             return NAN;
         }
     }
     return bearing;
 }
 类似的约束函数都在src/lib/geo/geo.c中，比如：
 __EXPORT float _wrap_180(float bearing);
 __EXPORT float _wrap_360(float bearing);
 __EXPORT float _wrap_pi(float bearing);
 __EXPORT float _wrap_2pi(float bearing);

         corr +=_q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;//n系到b系

         计算corr值等于单位化的旋转矩阵R（b系转n系）的转置（可以理解为 R（n系转b系））乘以（0,0，-mag_err），相当于机体坐标系绕地理坐标系N轴（Z轴）转动arctan（mag_earth(1), mag_earth(0)）度。

         关于磁还需要更深入的了解，看相关论文吧，一大推~~~

         ------------------------------------------------------------------------------------------------------------------------------

        加速度计修正

        首先就是把归一化的n系重力加速度通过旋转矩阵R左乘旋转到b系，即k为归一化的旋转矩阵R（b-e）的第三行，代码如下。

 // Accelerometer correction
     // Project 'k' unit vector of earth frame to body frame
     // Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));// n系到b系
     // Optimized version with dropped zeros
     Vector<3> k(
         2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
         2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
         (_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
     );

        上面这段代码是不是很熟悉，还记得mahony算法中的的计算过程么？！都是一样的，这里只是换种方式（C++）表达，不在赘述。

        下面这个比较重要：

        corr += (k %(_accel - _pos_acc).normalized()) * _w_accel。

        {k%（_accel“加速度计的测量值”-位移加速度）的单位化）<约等于重力加速度g>}*权重。

        关于这个“_accel-_pos_acc”的含义：

        总的受到合力的方向（_accel）减去机体加速度方向（_pos_acc）得到g的方向，即总加速度（加速度获取）减去机体运动加速度（第五部分）获取重力加速度，然后姿态矩阵的不是行就是列来与纯重力加速度来做叉积，算出误差。因为运动加速度是有害的干扰，必须减掉。算法的理论基础是[0,0,1]与姿态矩阵相乘。该差值获取的重力加速度的方向是导航坐标系下的z轴，加上运动加速度之后，总加速度的方向就不是与导航坐标系的天或地平行了，所以要消除这个误差，即“_accel-_pos_acc”。然后叉乘z轴向量得到误差，进行校准 。

        关于%运算符在vector.hpp中介绍了其原型：

 Vector<3> operator %(const Vector<3> &v) const {
         return Vector<3>(
                    data[1] * v.data[2] - data[2] * v.data[1],
                    data[2] * v.data[0] - data[0] * v.data[2],
                    data[0] * v.data[1] - data[1] * v.data[0]
                );
     }
 };

       “ %”其实就是求向量叉积，叉积公式如下。

        ---------------------------------------------------------------------------------------------------------------------------------

        _gyro_bias+= corr * (_w_gyro_bias * dt);PI控制器中的I（积分）效果。然后对_gyro_bias做约束处理。

 for (int i = 0; i < 3; i++) {
         _gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
     }

        对偏移量进行约束处理，源码写的相当好啊，简单易懂，其函数原型是：

 uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max)
 {
       return (val < min) ? min : ((val > max) ? max : val);
 }

        ---------------------------------------------------------------------------------------------------------------------------------

        最后就是使用修正的数据更新四元数，并把_rates和_gyro_bias置零便于下次调用时使用。

 _rates = _gyro + _gyro_bias; //得到经过修正后的角速度
     // Feed forward gyro
     corr += _rates;//PI控制器的体现
     // Apply correction to state
     _q += _q.derivative(corr) * dt;//736
     // Normalize quaternion
     _q.normalize();
     if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
           PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
         // Reset quaternion to last good state
         _q = q_last;
         _rates.zero();
         _gyro_bias.zero();
         return false;
     }
     return true;

        上面736行的_q += _q.derivative(corr) * dt非常重要，又用到了微分方程离散化的思想。以前讲过DCM矩阵更新过程中也是用到了该思想。先看看代码，有点怪，怪就怪在derivative（衍生物）这个名字上，平时一大推的论文和期刊上面都是用的omga *Q 的形式，而这里的代码实现确是用的Q * omga的形式，所以构造的4*4矩阵的每一列的符号就不一样了。

 /*** derivative*/
     const Quaternion derivative(const Vector<3> &w) {
         float dataQ[] = {
             data[0], -data[1], -data[2], -data[3],
             data[1],  data[0], -data[3],  data[2],
             data[2],  data[3],  data[0], -data[1],
             data[3], -data[2],  data[1],  data[0]
         };
         Matrix<4, 4> Q(dataQ);
         Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]);
         return Q * v * 0.5f;
     }

        这一部分理论基础在《捷联惯性导航技术》中有详细介绍，关于DCM随时间传递的推导过程、四元数随时间传递的推导以及DCM、欧拉角、四元数之间的相互关系都有详细的介绍。

        总结一下：corr包含磁力计修正、加速度计修正、（vision、mocap修正）、gyro中测量到的角速度偏转量，且因为corr为update函数中定义的变量，所以每次进入update函数中时会刷新corr变量的数据； _rate也会刷新其中的数据，含义为三个姿态角的角速度（修正后）； _q为外部定义的变量，在这个函数中只会+=不会重新赋值，如果计算出现错误会返回上一次计算出的_q。

        ---------------------------------------------------------------------------------------------------------------------------------

7、将_q转换成欧拉角euler并发布（532）

        终于从updata函数出来了，它还是相当重要的，主要就是它了，需要深入的理解透彻，下面就是些小角色了。Updata函出来就是直接用一更新的四元数（_q）求出对于的欧拉角，以便在控制过程中实现完美的控制，控制还是需要用直接明了的欧拉角。上源码：

 Vector<3> euler = _q.to_euler();
         struct vehicle_attitude_s att = {};
         att.timestamp = sensors.timestamp;
         att.roll = euler(0);//获取的欧拉角赋值给roll、pitch、yaw
         att.pitch = euler(1);
         att.yaw = euler(2);

         att.rollspeed = _rates(0); //获取roll、pitch、yaw得旋转速度
         att.pitchspeed = _rates(1);
         att.yawspeed = _rates(2);
         for (int i = 0; i < 3; i++) {
             att.g_comp[i] = _accel(i) - _pos_acc(i);
 //获取导航坐标系的重力加速度，前面已介绍过
         }

        比较重要的就是如何由四元数获取欧拉角：_q.to_euler()

 /**
      * create Euler angles vector from the quaternion
      */
     Vector<3> to_euler() const {
         return Vector<3>(
             atan2f(2.0f * (data[0] * data[1] + data[2] * data[3]), 1.0f - 2.0f * (data[1] * data[1] + data[2] * data[2])),
             asinf(2.0f * (data[0] * data[2] - data[3] * data[1])),
             atan2f(2.0f * (data[0] * data[3] + data[1] * data[2]), 1.0f - 2.0f * (data[2] * data[2] + data[3] * data[3]))
         );
     }

        下面的就比较好理解了，不在赘述。

 if (_att_pub == nullptr) {
             _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
         } else {
             orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
         }
         struct control_state_s ctrl_state = {};
         ctrl_state.timestamp = sensors.timestamp;
         /* Attitude quaternions for control state */
         ctrl_state.q[0] = _q(0);
         ctrl_state.q[1] = _q(1);
         ctrl_state.q[2] = _q(2);
         ctrl_state.q[3] = _q(3);
         /* Attitude rates for control state */
         ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0));
         ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1));
         ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
         /* Publish to control state topic */
         if (_ctrl_state_pub == nullptr) {
             _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
         } else {
             orb_publish(ORB_ID(control_state), _ctrl_state_pub, &ctrl_state);
         }

六、总结
        通过上面的分析对ardupilot源代码中的姿态解算算法有了深一步的了解，还有一部分就是关于mag的，还需要看一些论文，磁是最不容易处理的，极易受到外部干扰。还有就是加速度计的数据，对震动比较敏感，所以在无人机装机时需要在pixhawk下面安装减震板，做减震处理。
        基本的姿态解算和飞行控制靠陀螺仪和加速计等肯定可以实现，代码也比较好写，可以基于mahony的那套算法，硬件部分就是选定芯片，确定通信接口；然后移植一套免费的OS即可。但是难得就是飞行时的稳定性和对震动和噪声的处理问题，这些都是最细节最重要的部分，也是最难的部分。
        接下来就是关于姿态控制的了，至少一个多礼拜的时间才能搞明白吧；att_estimate、pos_estimate和att_control、pos_control这四个部分全部搞定需要下很大的功夫，慢慢搞吧，看论文吧，站在别人的肩膀上，加油~~~     
        感谢Mr、青龙等群友的帮助~~~