一、開篇
? ? ? ? 終于到ardupilot源代碼的姿態解算了，有了前期關于mahony姿態解算算法的基礎以后，理解源代碼的姿態解算算法就快多了，所有的東西都在腦海中初步有了一個框架；首先要做什么，然后再做什么，再然后捏~~~反正容易上手的。
? ? ? ? 2016.04.04日晚，別人都在嗨，而我卻在實驗室苦逼的工作著，今晚最大的收獲就是發現了“新大陸”-----“北航可靠飛行控制研究組”，其喜悅之情絕不亞于哥倫布發現新大陸。他們才是專業的啊，看看他們畢業生的去向，不是研究所就是出國深造，好吧，人家才是專業搞科研的，不食人間煙火，那么問題來了：DJI的員工都是在哪招來的呢？！哎，我是俗人一個，經濟基礎決定上層建筑，好好學習才能掙大錢。最近他們開設了一門課程《多旋翼飛行器設計與控制》，課程體系安排的非常好，現在更新到第四講了（聽北航一個博士說只有PPT沒有視頻，感謝Mallin的幫助，成功打入內部），PPT也足夠了，相當上檔次啊，課程到2016.06.30結束，正好可以把無人機的整個架構理解完，找工作去~~~
? ? ? ? 在下面的基礎知識部分先分享一部分“北航”的研究成果，特別是氣動方面的，以前一點概念都沒有，只看著超跑的流線型非常炫酷，不知其原因，特此記錄，大家共勉。
三、實驗平臺
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的數據：（有個特別的地方就是把陀螺儀的值先積分然后再微分，這個其實就是求平均_2016.05.27補充）
[plain] view plain
copy 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的數據。
[plain] view plain
copy 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函數。
[plain] view plain
copy 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值，其代表振動的幅度大小。
[plain] view plain
copy 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）
[plain] view plain
copy //?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;??????????}</span><span?style="font-size:14px;">??
5、位置加速度獲取（position，注意最后在修正時會用到該處的_pos_acc）（484）
? ? ? ? 首先檢測是否配置了自動磁偏角獲取，如果配置了則用當前的經緯度（longitude and latitude）通過get_mag_declination(_gpos.lat,_gpos.lon)函數獲取當前位置的磁偏角（magnetic declination），實現過程就是一系列的算算算，自己跟蹤源碼看吧，用地面站校準羅盤的應該比較熟悉這個。然后就是獲取機體的速度，通過速度計算機體的加速度。
[plain] view plain
copy 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函數初始化，源碼如下。
[plain] view plain
copy 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()函數賦值。
[plain] view plain
copy /**???*?set?row?from?vector???*/??void?set_row(unsigned?int?row,?const?Vector<N>?v)?{??????for?(unsigned?i?=?0;?i?<?N;?i++)?{??????????data[row][i]?=?v.data[i];??????}??}??
? ? ? ? 第一行賦值i 向量指向北，第二行賦值j 向量指向東，第三行賦值k向量指向DOWN。然后就是把DCM轉換為四元數_q （使用from_dcm），并歸一化四元數，一定要保持歸一化的思想。來一個比較牛掰的求范數的倒數的快速算法（mahony的算法實現用到的）：
[plain] view plain
copy 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時比較好理解，別的情況被簡寫的看不透了。后續給出原始代碼便于理解。
[plain] view plain
copy tr?=?dcm.data[0][0]?+?dcm.data[1][1]?+?dcm.data[2][2]??????如果?tr>0??float?s?=?sqrtf(tr?+?1.0f);??data[0]?=?s?*?0.5f;??s?=?0.5f?/?s;??data[1]?=?(dcm.data[2][1]?-?dcm.data[1][2])?*?s;??data[2]?=?(dcm.data[0][2]?-?dcm.data[2][0])?*?s;??data[3]?=?(dcm.data[1][0]?-?dcm.data[0][1])?*?s;??
? ? ? ? 其他情況去看代碼吧，不好解釋。然后_q 單位化結束初始化。PS：另外根據DCM求取四元數的算法是參考MartinJohn Baker的，就是上述的原始版，該算法在AP_Math/quaternion.cpp中，鏈接：
http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
? ? ? ? 源碼如下。
[plain] view plain
copy 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。
[plain] view plain
copy 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利用磁力計修正。
[plain] view plain
copy 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系的轉換，使用左乘。
[plain] view plain
copy /**???????*?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的函數，源碼如下。
[plain] view plain
copy __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）的第三行，代碼如下。
[plain] view plain
copy //?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中介紹了其原型：
[plain] view plain
copy 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做約束處理。
[plain] view plain
copy for?(int?i?=?0;?i?<?3;?i++)?{??????????_gyro_bias(i)?=?math::constrain(_gyro_bias(i),?-_bias_max,?_bias_max);??????}??
? ? ? ? 對偏移量進行約束處理，源碼寫的相當好啊，簡單易懂，其函數原型是：
[plain] view plain
copy uint64_t?__EXPORT?constrain(uint64_t?val,?uint64_t?min,?uint64_t?max)??{????????return?(val?<?min)???min?:?((val?>?max)???max?:?val);??}??
? ? ? ? ---------------------------------------------------------------------------------------------------------------------------------
? ? ? ? 最后就是使用修正的數據更新四元數，并把_rates和_gyro_bias置零便于下次調用時使用。
[plain] view plain
copy _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矩陣的每一列的符號就不一樣了。
[plain] view plain
copy /***?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）求出對于的歐拉角，以便在控制過程中實現完美的控制，控制還是需要用直接明了的歐拉角。上源碼：
[plain] view plain
copy 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()
[plain] view plain
copy /**???????*?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]))??????????);??????}??
? ? ? ? 下面的就比較好理解了，不在贅述。
[plain] view plain
copy 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、青龍等群友的幫助~~~
版權聲明：. https://blog.csdn.net/qq_21842557/article/details/51058206
總結
以上是生活随笔為你收集整理的Pixhawk之姿态解算篇（3）_源码姿态解算算法分析的全部內容，希望文章能夠幫你解決所遇到的問題。
如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。
