初學PX4之飛控演算法

07-14

通知：如果你對本站無人機文章不熟悉，建議查看無人機學習概覽！！！注意：基於參考原因，本文參雜了APM的演算法分析。

本篇文章首先簡述了下px4和apm調用姿態相關應用程序出處，然後對APM的DCM姿態解算演算法參考的英文文檔進行了翻譯與概括，並結合源代碼予以分析，在此之前，分析了starlino的DCM，並進行了matlab的實現，因為它更加利於理解。後段時間會對px4的四元數姿態解算進行分析。姿態控制部分描述了串級PID在APM里的實現流程，同樣後期會完善對px4的分析。最後針對自己平時使用的一些調試技巧進行了總結。

姿態出處分析

下面看下重要的一個腳本/etc/init.d/rc.mc_apps，可以知道姿態估計用的是attitude_estimator_q和position_estimator_inav，用戶也可以選擇local_position_estimator、ekf2，而姿態控制應用為mc_att_control和mc_pos_control。
#!nshif param compare INAV_ENABLED 1then attitude_estimator_q start position_estimator_inav startelse if param compare LPE_ENABLED 1 then attitude_estimator_q start local_position_estimator start else ekf2 start fifiif mc_att_control startthenelse # try the multiplatform version mc_att_control_m startfiif mc_pos_control startthenelse # try the multiplatform version mc_pos_control_m startfi...
而在ardupilot中，姿態解算與控制演算法在ArduCopter.cpp的fast_loop任務中以400Hz的頻率運行。
// Main loop - 400hzvoid Copter::fast_loop(){ // IMU DCM Algorithm // -------------------- read_AHRS(); ...}void Copter::read_AHRS(void){ ... ahrs.update();}

了解了上面的源碼出處後，下面將分具體應用進行分析。

姿態估算DCM_tutorial

imu_guide/imu_guide中文翻譯/dcm_tutorial/wiki資料查詢/該部分演算法源碼參考將該演算法轉換為了matlab實現，想了解的可以查看我的github里的DCM工程，能夠更好的理解演算法，另外，該matlab實現還有一定的bug，希望各位大神的pull request~這部分翻譯自dcm_tutorial，並結合源碼進行分析，可作為下部分DCM理論介紹的基礎哦，所以建議先將這部分看完再往下看～

DCM矩陣：

設機體坐標係為Oxyz，與機體坐標系同x,y,z方向的單位向量為i, j, k。地理坐標係為OXYZ，同理地理坐標系的單位向量為 I, J, K。共同原點為O。如圖

I
$^{G}$

= {1,0,0}

^{T}

, J

^{G}

={0,1,0}

^{T}

, K

^{G}

= {0,0,1}

^{T}

^{B}

= {1,0,0}

^{T}

, j

^{B}

={0,1,0}

^{T}

, k

^{B}

= {0,0,1}

^{T}

下面將i，j，k向量用地理坐標系表示，首先以i作為一個例子。i

^{G}

= {i

_{x}

^{G}

, i

_{y}

^{G}

, i

_{z}

^{G}

}

^{T}

其中i

_{x}

^{G}

表示的是i向量在地理坐標系X軸上的投影，即i

_{x}

^{G}

= |i| cos(X,i) = cos(I,i)在這個式子中，|i|是i單位向量的範式（長度），cos(I,i)是向量I和向量i夾角的餘弦，因此可以這樣寫：i

_{x}

^{G}

= cos(I,i) = |I||i| cos(I,i) = I.i在這個式子中，I.i是向量I和向量i的點積，由於只是計算點積，我們並不關心向量是在哪個坐標系中測量的，只要都是以同一個坐標系表示即可。所以： I.i = I

^{B}

^{B}

= I

^{G}

^{G}

= cos(I

^{B}

^{B}

) = cos(I

^{G}

^{G}

)同樣的：i

_{y}

^{G}

= J.i , i

_{z}

^{G}

=K.i所以i向量可以用地理坐標系表示為：i

^{G}

= { I.i, J.i, K.i}

^{T}

同樣的，j，k向量可以表示為：j

^{G}

= { I.j, J.j, K.j}

^{T}

, k

^{G}

= { I.k, J.k, K.k}

^{T}

將機體坐標i，j，k的地理坐標表示以矩陣的形式表示為：

這就是方向餘弦矩陣，它是由機體坐標系和地理坐標系向量所有兩兩向量組合的夾角的餘弦組成，可以算出共有9

\times

9種。下一節的DCM理論裡面有另外一種推理DCM的方法，建議交互思考！

而將地理坐標系在機體坐標系中表示與將機體坐標系在地理坐標系中表示是對稱的，所以只需簡單交換I, J, K 和 i, j, k即可。I
$^{B}$

= { I.i, I.j, I.k}

^{T}

, J

^{B}

= { J.i, J.j, J.k}

^{T}

, K

^{B}

= { K.i, K.j, K.k}

^{T}

轉化為矩陣形式為：

很容易可以發現：DCM

^{B}

= (DCM

^{G}

)

^{T}

or DCM

^{G}

= (DCM

^{B}

)

^{T}

，換句話說，這兩個矩陣是可以相互轉換的。也可以發現：DCM

^{B}

. DCM

^{G}

= (DCM

^{G}

)

^{T}

.DCM

^{G}

= DCM

^{B}

. (DCM

^{B}

)

^{T}

= I

_{3}

其中I

_{3}

為3

\times

3的單位矩陣，換句話說，DCM矩陣是正交矩陣。證明如下：

證明這個我們需要知道這些特性：i

^{G T}

. i

^{G}

= | i

^{G}

|| i

^{G}

|cos(0) = 1， i

^{G T}

. j

^{G}

= 0，因為i和j是正交的。

方向餘弦矩陣也稱為旋轉矩陣，如果知道機體坐標，則可以算出任意向量的地理坐標（反之同理），下面以機體坐標系向量使用DCM算出地理坐標作為例子進行推理：r
$^{B}$

= { r

_{x}

^{B}

, r

_{y}

^{B}

, r

_{z}

^{B}

}

^{T}

而r

^{G}

= { r

_{x}

^{G}

, r

_{y}

^{G}

, r

_{z}

^{G}

}

^{T}

現在讓我們分析第一個坐標r

_{x}

^{G}

：r

_{x}

^{G}

= | r

^{G}

| cos(I

^{G}

^{G}

)，由於坐標系旋轉，向量的大小，夾角都不會變，故有：| r

^{G}

| = | r

^{B}

| , | I

^{G}

| = | I

^{B}

| = 1 ， cos(I

^{G}

^{G}

) = cos(I

^{B}

^{B}

)，所以：r

_{x}

^{G}

= | r

^{G}

| cos(I

^{G}

^{G}

) = | I

^{B}

|| r

^{B}

| cos(I

^{B}

^{B}

) = I

^{B}

. r

^{B}

= I

^{B}

. { r

_{x}

^{B}

, r

_{y}

^{B}

, r

_{z}

^{B}

}

^{T}

由上可知，r

^{B}

= { r

_{x}

^{B}

, r

_{y}

^{B}

, r

_{z}

^{B}

}

^{T}

，替換得：r

_{x}

^{G}

= I

^{B}

. r

^{B}

= { I.i, I.j, I.k}

^{T}

. { r

_{x}

^{B}

, r

_{y}

^{B}

, r

_{z}

^{B}

}

^{T}

= r

_{x}

^{B}

I.i + r

_{y}

^{B}

I.j + r

_{z}

^{B}

I.k同樣的思路：r

_{y}

^{G}

= rx

^{B}

J.i + ry

^{B}

J.j + rz

^{B}

J.krz

^{G}

= rx

^{B}

K.i + ry

^{B}

K.j + rz

^{B}

K.k轉化為矩陣形式為：

得證。同樣的思路可以證明：

也可以這樣證明：DCM

^{B}

^{G}

= DCM

^{B}

DCM

^{G}

^{B}

= DCM

^{G T}

DCM

^{G}

^{B}

= I

_{3}

^{B}

= r

^{B}

角速度：

如下圖所示，r為任意的旋轉向量，t時刻的坐標為r(t)。時間間隔dt後：r = r (t) , r』= r (t+dt) and dr = r』 – r。

dt時間後向量r繞著與單位向量u同向的軸旋轉了d
$θ$

，停到了向量r"的位置。其中u垂直與旋轉的機身，因此u正交於r與r"，圖中顯示了u與u"，它們與r和r『的叉乘結果方向相同。故有u = (r x r』) / |r x r』| = (r x r』) / (|r|| r』|sin(dθ)) = (r x r』) / (|r|

^{2}

sin(dθ))由於旋轉並不改變向量的長度，因此有| r』| = |r|。向量r的線速度可以表示如下：v = dr / dt = ( r』 – r) / dt當dθ → 0時，向量r和dr的夾角

α

可通過r，r"和dr組成的等腰三角形計算：α = (π – dθ) / 2 當dθ→ 0時，α → π/2。這告訴我們，當dt → 0時，r垂直於dr，因此r ⊥ v （v和dr的方向是一致的）。現在定義角速度向量，其中反應了角度的變化率和旋轉軸方向。w = (dθ/dt ) u

下面分析w和v之間的關係：w = (dθ/dt ) u = (dθ/dt ) (r x r』) / (|r|
$^{2}$

sin(dθ))當dt → 0時，dθ → 0，因此sin(dθ) ≈ dθ。化簡得：w = (r x r』) / (|r|

^{2}

dt)現在由於 r』 = r + dr , dr/dt = v , r x r = 0，利用叉乘的加法分配率可得：w = (r x (r + dr)) / (|r|

^{2}

dt) = (r x r + r x dr)) / (|r|

^{2}

dt) = r x (dr/dt) / |r|

^{2}

最後得出：w = r x v / |r|

^{2}

下面反向推理證明v = w x r利用向量的三重積公式：(a x b) x c = (a.c)b – (b.c)a，以及v和r是垂直的，所以v.r = 0w x r = (r x v / |r|

^{2}

-) x r = (r x v) x r / |r|

^{2}

- = ((r.r) v + (v.r)r) / |r|

^{2}

- = ( |r|

^{2}

- v + 0) |r|

^{2}

= v得證。

陀螺儀及角速度向量

如果我們定期獲取陀螺儀的值，時間間隔為dt，那麼陀螺儀將會告訴我們在這段時間，地球繞陀螺儀各軸旋轉的度數。dθ

_{x}

= w

_{x}

dt，dθ

_{y}

= w

_{y}

dt，dθ

_{z}

= w

_{z}

dt其中w

_{x}

= w

_{x}

i = {w

_{x}

, 0 , 0 }

^{T}

, w

_{y}

= w

_{y}

j = { 0 , w

_{y}

, 0 }

^{T}

, w

_{z}

= w

_{z}

k = { 0 , 0, w

_{z}

}

^{T}

每次旋轉都會產生線性的位移dr

_{1}

= dt v

_{1}

= dt (w

_{x}

x r) ; dr

_{2}

= dt v

_{2}

= dt (w

_{y}

x r) ; dr

_{3}

= dt v

_{3}

= dt (w

_{z}

x r) .矢量相加：dr = dr

_{1}

+ dr

_{2}

+ dr

_{3}

= dt (w

_{x}

x r + w

_{y}

x r + w

_{z}

x r) = dt (w

_{x}

+ w

_{y}

+ w

_{z}

) x r因此線速度可以表示為：v = dr/dt = (w

_{x}

+ w

_{y}

+ w

_{z}

) x r = w x r這裡的條件是dt很小才能這麼推理，也就是說，dt越大誤差也就越大。

基於6DOF或者9DOF的IMU感測器DCM互補濾波演算法

科普：6DOF由三軸陀螺儀和三軸加速度計組成，9DOF由三軸磁力計、三軸陀螺儀和三軸加速度計組成。定義右手地理坐標系：I指向北方，K指向頂部，J指向西方。

IMU組成機體坐標系，IMU包括陀螺儀、加速度計等。加速度計能感應重力，重力向量指向地心，與頂部向量K
$^{B}$

方向相反，假如加速度計輸出為A = {A

_{x}

, A

_{y}

, A

_{z}

}，則K

^{B}

= –A。磁力計與加速度計相似，除了磁力計可以感應地理的北方以外，假設正確的磁力計輸出為M = {M

_{x}

, M

_{y}

, M

_{z}

}，由於I

^{B}

是指向北方，因此I

^{B}

= M.現在可以計算出J

^{B}

= K

^{B}

x I

^{B}

。所以加速度計和磁力計就能單獨的給出DCM矩陣：DCM

^{G}

= DCM

^{B T}

= [I

^{B}

, J

^{B}

, K

^{B}

]

^{T}

DCM矩陣能轉換機體坐標系中的任意向量到地理坐標系中：r

^{G}

= DCM

^{G}

^{B}

加速度計和磁力計都需要初始化校正和糾錯。陀螺儀沒有絕對的方向感，比如它不知道北方和頂部在哪裡，但加速度計和磁力計知道，因此當我們知道t時刻的方向，矩陣形式表示為DCM(t)，那麼我們可以用陀螺儀算出更精確的方向DCM(t+1)，所以這就是從帶有噪音的加速度計或者有磁場干擾的磁力計估算出來的姿態。事實是我們可以利用陀螺儀、加速度計和磁力計融合來估算姿態。下面將介紹這種演算法：地理坐標系表示的DCM矩陣如下：

該DCM矩陣的各行為I

^{B}

, J

^{B}

, K

^{B}

向量。我們會將重心放在K

^{B}

（由加速度計測定），I

^{B}

（由磁力計測定）上。 J

^{B}

可以由J

^{B}

= K

^{B}

x I

^{B}

計算出來。假設機體坐標的頂部向量在t

_{0}

時刻表示為K

^{B}

_{0}

，陀螺儀的輸出為 w = {w

_{x}

, w

_{y}

, w

_{z}

}，一段時間後，該頂部向量表示為K

^{B}

_{1} G

^{B}

_{1} G

≈ K

^{B}

_{0}

+ dt v = K

^{B}

_{0}

+ dt (w

_{g}

x K

^{B}

_{0}

) = K

^{B}

_{0}

+ ( dθ

_{g}

x K

^{B}

_{0}

)其中dθ

_{g}

= w

_{g}

dt為三個軸角度的變化向量，這意味這在dt時間內K

^{B}

向量在3個軸改變的角度。w

_{g}

為陀螺儀測得的角速度。程序如下：

//--------------- //dcmEst //--------------- //gyro rate direction is usually specified (in datasheets) as the device"s(body"s) rotation //about a fixed earth"s (global) frame, if we look from the perspective of device then //the global vectors (I,K,J) rotation direction will be the inverse float w[3]; //gyro rates (angular velocity of a global vector in local coordinates) w[0] = -getGyroOutput(1); //rotation rate about accelerometer"s X axis (GY output) in rad/ms w[1] = -getGyroOutput(0); //rotation rate about accelerometer"s Y axis (GX output) in rad/ms w[2] = -getGyroOutput(2); //rotation rate about accelerometer"s Z axis (GZ output) in rad/ms for(i=0;i<3;i++){ w[i] *= imu_interval_ms; //scale by elapsed time to get angle in radians //compute weighted average with the accelerometer correction vector w[i] = (w[i] + ACC_WEIGHT*wA[i] + MAG_WEIGHT*wM[i])/(1.0+ACC_WEIGHT+MAG_WEIGHT); }

很顯然，還可以通過另外的方式估算K

^{B}

。如加速度估算值K

^{B}

_{1 A}

，如下推理：w-

_{a}

= K

^{B}

_{0}

x v

_{a}

/ | K

^{B}

_{0}

|2- 這個在上面的角速度部分得到了證實。其中 v

_{a}

= (K

^{B}

_{1 A}

- – K

^{B}

_{0}

) / dt，v

_{a}

為K

^{B}

_{0}

的線速度，且| K

^{B}

_{0}

^{2}

-- = 1 ，故可以這麼計算：dθ

_{a}

-= dt w

_{a}

= K

^{B}

_{0}

x (K

^{B}

_{1 A} -

– K

^{B}

_{0}

) = K

^{B}

_{0}

x K

^{B}

_{1 A} -

– K

^{B}

_{0}

x K

^{B}

_{0}

= K

^{B}

_{0}

x K

^{B}

_{1 A} -

- 0 = K

^{B}

_{0}

x K

^{B}

_{1 A} -

程序如下：

//--------------- //Acelerometer //--------------- //Accelerometer measures gravity vector G in body coordinate system //Gravity vector is the reverse of K unity vector of global system expressed in local coordinates //K vector coincides with the z coordinate of body"s i,j,k vectors expressed in global coordinates (K.i , K.j, K.k) //Acc can estimate global K vector(zenith) measured in body"s coordinate systems (the reverse of gravitation vector) Kacc[0] = -getAcclOutput(0); Kacc[1] = -getAcclOutput(1); Kacc[2] = -getAcclOutput(2); vector3d_normalize(Kacc); //calculate correction vector to bring dcmEst"s K vector closer to Acc vector (K vector according to accelerometer) float wA[3]; vector3d_cross(dcmEst[2],Kacc,wA); // wA = Kgyro x Kacc , rotation needed to bring Kacc to Kgyro

可以通過融合K

^{B}

_{1 A}

和K

^{B}

_{1 G}

計算新的估算值K

^{B}

1 ，首先通過求dθa和dθg的加權平均來求dθ：dθ = (s

_{a}

dθ

_{a}

+ s

_{g}

dθ

_{g}

) / (s

_{a}

+ s

_{g}

-)為什麼要求dθ，因為可以同時求得：K

^{B}

_{1}

≈ K

^{B}

_{0}

+ ( dθ x K

^{B}

_{0}

^{B}

_{1}

≈ I

^{B}

_{0}

+ ( dθ x I

^{B}

_{0}

^{B}

_{1}

≈ J

^{B}

_{0}

+ ( dθ x J

^{B}

_{0}

)由於I

^{B}

, J

^{B}

, K

^{B}

是相互聯繫的，所以跟蹤相同的dθ。到目前為止，我們都沒有提及磁力計，一個原因是6DOF的IMU是沒有磁力計的，這樣也可以使用，只是航向會產生飄移，因此我們可以使用一個虛擬的磁力計，代碼中會有體現的。磁力計與加速度計相似：dθ

_{m}

-= dt w

_{m}

= I

^{B}

_{0}

x (I

^{B}

_{1 M} -

– I

^{B}

_{0}

)算入加權平均為：dθ = (s

_{a}

dθ

_{a}

+ s

_{g}

dθ

_{g}

+ s

_{m}

dθ

_{m}

) / (s

_{a}

+ s

_{g}

+- s

_{m}

)更新DCM矩陣：I

^{B}

_{1}

≈ I

^{B}

_{0}

+ ( dθ x I

^{B}

_{0}

) , K

^{B}

_{1}

≈ K

^{B}

_{0}

+ ( dθ x K

^{B}

_{0}

) 和 J

^{B}

_{1}

≈ J

^{B}

_{0}

+ ( dθ x J

^{B}

_{0}

)下面通過計算J

^{B}

_{1}

= K

^{B}

_{1}

x I

^{B}

_{1}

，判斷估算後的值K

^{B}

_{1}

是否垂直I

^{B}

_{1}

。

了確保估算後的值是否還是正交的，如下圖，假設向量a，b是幾乎垂直的，但不是90°，我們可以找到一個向量b』與a垂直，這個b』向量可以通過求 c = a x b，再求 b』 = c x a 得到，可以看出b"是正交於a和c的，因此b"是校正後的向量。

利用向量的三重積公式展開，且a.a = |a| = 1：b』 = c x a = (a x b) x a = –a (a.b) + b(a.a) = b – a (a.b) = b + d，其中d = – a (a.b)是校正量，平行於a，方向取決於a和b的夾角。上面的情況是a向量固定，b向量得到校正，下面分析對稱的情況，a得到校正，b固定：a』 = a – b (b.a) = a – b (a.b) = a + e，其中e = – b (a.b)再下面考慮這兩個向量都有誤差，都得到一半的校正得：a』 = a – b (a.b) / 2b』 = b – a (a.b) / 2所以得到一個相對簡單的公式：Err = (a.b)/2a』 = a – Err * bb』 = b – Err * a現在我們可以更新DCM矩陣的 IB1, JB1向量。Err = ( I
$^{B}$

_{1}

. J

^{B}

_{1}

) / 2I

^{B}

_{1}

』 = I

^{B}

_{1}

– Err * J

^{B}

_{1}

^{B}

_{1}

』 = J

^{B}

_{1}

– Err * I

^{B}

_{1}

^{B}

_{1}

』』 = Normalize[I

^{B}

_{1}

』]J

^{B}

_{1}

』』 = Normalize[J

^{B}

_{1}

』]K

^{B}

_{1}

』』 = I

^{B}

_{1}

』』 x J

^{B}

_{1}

』』其中Normalize[a] = a / |a|，單位化。

代碼如下：

//bring dcm matrix in order - adjust values to make orthonormal (or at least closer to orthonormal)void dcm_orthonormalize(float dcm[3][3]){ //err = X . Y , X = X - err/2 * Y , Y = Y - err/2 * X (DCMDraft2 Eqn.19) float err = vector3d_dot((float*)(dcm[0]),(float*)(dcm[1])); float delta[2][3]; vector3d_scale(-err/2,(float*)(dcm[1]),(float*)(delta[0])); vector3d_scale(-err/2,(float*)(dcm[0]),(float*)(delta[1])); vector3d_add((float*)(dcm[0]),(float*)(delta[0]),(float*)(dcm[0])); vector3d_add((float*)(dcm[1]),(float*)(delta[1]),(float*)(dcm[1])); //Z = X x Y (DCMDraft2 Eqn. 20) , vector3d_cross((float*)(dcm[0]),(float*)(dcm[1]),(float*)(dcm[2])); //re-nomralization vector3d_normalize((float*)(dcm[0])); vector3d_normalize((float*)(dcm[1])); vector3d_normalize((float*)(dcm[2]));}

DCM理論

注意：這部分屬於APM源碼里px4姿態解算部分。資料翻譯解讀自DMCDraft2.pdf（翻譯不妥請諒解，歡迎提意見，另外該理論文檔已啟動翻譯，如果你想參與請點擊這裡），並結合文檔分析了APM的姿態源碼部分，目前還有drift_correction函數未進行整理！

前言

使用矩陣來控制和導航，元素包括陀螺儀，加速度計和gps信息。

總的來說，DCM工作如下：

陀螺儀作為主要的方向信息來源，通過整合一個非線性微分運動方程，表明飛機方向的變化率與旋轉速率及它當前的方向之間的關係。

意識到積分過程中的積分誤差將漸漸的違反DCM必須滿足的正交約束，我們對矩陣的元素進行規則的小調整滿足約束。

由於數字誤差，陀螺儀漂移，陀螺儀偏移量將逐漸積累在DCM中的元素的誤差，我們使用參考向量來檢測誤差，以及在檢測到的誤差和和第一步的陀螺儀輸入中加一個比例積分負反饋控制器來在建立之前消除誤差。gps是用來檢測偏航誤差，加速度計被用來檢測俯仰和滾動。整個過程如下：

代碼實現概覽：

// Integrate the DCM matrix using gyro inputs matrix_update(delta_t); // Normalize the DCM matrix normalize(); // Perform drift correction drift_correction(delta_t);

方向餘弦矩陣介紹

所有這一切都與旋轉有關。

有幾種方法可以做到，比如旋轉矩陣和四元數。這兩種方法在實現上具有相似的地方，都是盡量準確的表示旋轉。四元數只需要4個值，而旋轉矩陣需要9個，在這方面四元數具有優勢。而旋轉矩陣很適合用來導航和控制。

旋轉矩陣用來描述一個坐標系相對於另一個坐標系的方向。在一個系統中的向量可以通過乘以旋轉矩陣轉變到另一個系統中，如果是相反方向旋轉則乘以旋轉矩陣的逆矩陣，也是它的轉置（交換行和列）。單位向量在控制和導航運算中將非常有用，因為它們的長度為1。因此他們能被用於交積和叉積中來獲得各種正弦或餘弦角。

隨著飛機的飛行，我們可以用位置（重心的移動）和朝向（繞著重心方向的變化）了描述它的運動，類似這種變換我們稱為剛體變換。通過指定一個軸的旋轉來描述其相對於地球的方向。例如將飛機開始放在一個標準方向，然後將其旋轉，它將指向另外一個實際的方向，也就是說任何其他的方向都可以通過標準方向的旋轉描述。旋轉組是所有可能的旋轉的組。它被稱為一組，因為在該組中的任何2個旋轉可以組成一個組中的另一個旋轉，每一個旋轉有一個逆旋轉。這裡有一個單位旋轉。旋轉組應該得到重視的原因是，你能通過最少的近似來在各個方向控制和導航飛機，包括各種特技。基本的想法是，定義了你的飛機的方向的旋轉矩陣，可以通過結合描述旋轉運動學的非線性微分方程得到。這個結合可以通過一系列的旋轉組合完成，也就是兩矩陣相乘，這是兩個矩陣依次執行的結果。然而，數值積分引入的數值誤差，並不會產生與符號積分相同的結果。精確的陀螺儀信號的符號積分將產生完全正確的旋轉矩陣。數值積分，即使我們有精確的陀螺儀信號，也會引入2種數值誤差：

積分誤差。數值積分採用有限時間步長和在有限採樣速率下採樣數據。因為是假定在時間步長內旋轉速度是恆定的，這將引入了一個與旋轉加速度成比例的誤差。

量化誤差。不管你用什麼代表值，數字表示是有限的，所以有一個量化誤差，從模數轉換開始，以及所有計算沒有保留結果所有位時。

旋轉矩陣的一個關鍵特性是它的正交性，這意味著如果2個向量在一個參照系中是垂直的，它們在每一個參照系中都是垂直的。另外，在每一個參照系中向量的長度是一樣的。數值誤差可能違反此特性。在許多空間系統中,利用方向餘弦矩陣把矢量從一個笛卡爾坐標系變換到另一個笛卡爾坐標系。理想的方向餘弦矩陣應當是正交的,而實際上,通過計算得到的矩陣由於種種誤差(如計算方法誤差、舍入誤差等)而失去了正交性,造成變換誤差,影響系統精度。於是有必要按某種最優方式,恢復其正交性。矩陣正交化的迭代法有多種,但都計算較繁、運算量大。對於需要把計算得到的方向餘弦矩陣周期性地正交化的場合(如捷聯式慣導系統),大的運算量將給計算機實時計算帶來困難。例如，即使行和列都應該代表單位向量，它們的大小應該等於1，但數值誤差可能導致它們變得更小或更大。最終他們可以縮小到零，或去無限。行和列應該是垂直於彼此，數值誤差可能導致他們「傾斜」到對方，如下圖所示：

旋轉矩陣有9個元素。實際上，只有3個是獨立的。旋轉矩陣的正交特性在數學術語方面意味著矩陣的任何一對行或列都是垂直的。並且在每個列（或行）的元素的平方和等於1。所以這九個元素中有六個約束條件。

反對稱矩陣定義是：A=-A』（A的轉置前加負號），它的第Ⅰ行和第Ⅰ列各數絕對值相等，符號相反。且主對角線上的元素為均為零。一個小的旋轉可以用如下的反對稱矩陣來描述：

在我們的例子中，運動學與剛體旋轉的含義有關。它的結果是一個非線性微分方程，描述了剛體在其向量旋轉速度方面的時間演化。方向餘弦矩陣都是關於運動學的。

控制和導航可以在笛卡爾坐標系使用DCM完成叉積和點積運算。下面是具體步驟：

要控制飛機的俯仰，你需要知道這架飛機的俯仰姿態，你可以通過把飛機的翻滾軸與地面垂直做點積。

要控制飛機的翻滾，你需要知道這架飛機的傾斜姿態，你可以通過把飛機的俯仰軸與地面垂直做點積。

要航向，你需要知道你這架飛機相對於你想要去的方向的偏航姿態，可以通過飛機的翻滾軸與想要去的方向的向量做叉積得到。如果是去相反的方向，則是點積運算。

判斷飛機是否倒過來，可以通過判斷飛機偏航軸與垂直的點積符號，如果小於0，則是朝下的。

計算飛機繞垂直軸的旋轉速度，將陀螺儀的旋轉矢量轉換為地理參考坐標系，然後與垂直軸做點積。

下面將進行深入的理論研究。

確定一個合適的坐標系統描述飛機的運動是必要的。對於大多數處理飛機運動的問題，採用了雙坐標系。一個坐標系是固定在地球上的，可以被認為是是一個慣性坐標系，是為了飛機運動分析的目的。另一個坐標系是固定在飛機上的，被稱為機體坐標系。圖2顯示了兩右手坐標系：

其中 xe、ye、ze 是地球坐標系統,ze 指向地心,xe 指向正東方,ye 指向正北方;xb、yb、zb 為機體坐標系。

飛機的方向經常被描述為三個連續的旋轉，其順序是重要的。旋轉角被稱為歐拉角。假設機體坐標如下：

進行如下的旋轉就可以得到上面圖2的結果：

分析：第一步:假設我站在機體坐標中,我需要通過先繞 Xb 軸旋轉
$Φ$

，再旋轉 Yb 軸旋轉

θ

，最後繞 Zb 軸旋轉

ψ

，回到地球坐標系;先求出每次旋轉的矩陣。如果繞機體 X 軸旋轉的角度為

Φ

，那麼

這裡是怎麼得來的呢？先說一下什麼是旋轉矩陣？如下圖所示，我們假設最開始空間的坐標系也就是機體坐標系X

_{A}

，Y

_{A}

，Z

_{A}

就是笛卡爾坐標系，這樣我們得到空間A的矩陣V

_{A}

={X

_{A}

，Y

_{A}

，Z

_{A}

}

^{T}

，其實也可以看做是單位陣E。進過旋轉後，空間A的三個坐標系變成了圖1中紅色的三個坐標系X

_{B}

，Y

_{B}

，Z

_{B}

，得到空間B的矩陣V

_{B}

={X

_{B}

，Y

_{B}

，Z

_{B}

}

^{T}

。我們將兩個空間聯繫起來可以得到V

_{B}

=R·V

_{A}

，這裡R就是我們所說的旋轉矩陣。

由於X

_{A}

={1,0,0}

^{T}

，Y

_{A}

={0,1,0}

^{T}

，Z

_{A}

={0,0,1}

^{T}

，結合下圖可以看出，旋轉矩陣R就是由X

_{B}

，Y

_{B}

，Z

_{B}

三個向量組成的。講到這裡，大家應該會發現旋轉矩陣R滿足第一個條件，因為單位向量無論怎麼旋轉長度肯定不會變而且向量之間的正交性質也不會變。那麼旋轉矩陣就是正交陣！不過這還不能說明問題，下面我更進一步利用數學公式進行證明。

進一步討論之前，我們先說兩點數學知識。（1）點乘（dot product）的幾何意義：如下圖，我們從點乘的公式可以得到α·β相當於β的模乘上α在β上投影的模，所以當|β|=1時，α·β就是指α在β上投影的模。這一點在下面的內容中非常重要，之所以叫餘弦矩陣的原因就是這個。（2）旋轉矩陣逆的幾何意思：這個比較抽象，不過也好理解。旋轉矩陣相當於把一個向量（空間）旋轉成新的向量（空間），那麼逆可以理解為由新的向量（空間）轉回原來的向量（空間）。（3）向量是特殊的矩陣，只有一行或一列的矩陣稱為向量。向量有叉乘和點乘。矩陣也有，但意義不一樣，矩陣還有反對稱，逆矩陣等。

所以上面的公式解析如下：

同理，其他方向的旋轉計算如下：如果繞機體 Y 軸旋轉的角度為

θ

，那麼

如果繞機體 Z 軸旋轉的角度為

ψ

，那麼

第二步:由於站在機體坐標上需要按照 X->Y->Z 軸的順序,經過 3 次旋轉,才能回到地球坐標系;反過來如果站在地球坐標系,則需要經過 Z->Y->X 的三次旋轉才能到達機體坐標系。因此我們可以列出從地球坐標繫到機體坐標系的DCM矩陣，換句話說此DCM矩陣就是機體坐標在地理坐標系中的表示，其中

Φ, θ, ψ

為機體在地理坐標系中的姿態角。

L (Φ, θ, ψ) = L (ψ) ? L (θ) ? L (Φ);

矩陣的乘法計算得：

方向餘弦矩陣：向量的某些類型，如方向，速度，加速度，和轉換，（動作）可以轉化為旋轉參考系中的一個3x3的矩陣。我們感興趣的是機體參考系和地面參考系。它可以乘以一個向量的方向餘弦矩陣旋轉：

由上面的分析可知，方向餘弦矩陣與歐拉角之間的關係為：

方程1方程2表明了如何將機體坐標系中測得的向量轉換的地理坐標系中。方程1是以方向餘弦角的形式，而2為歐拉角。

以上整個求解過程是對 matrix3.cpp 代碼中 from_euler 函數的解析：

// create a rotation matrix given some euler angles// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdftemplate <typename T>void Matrix3<T>::from_euler(float roll, float pitch, float yaw){ float cp = cosf(pitch);//pitch 表示俯仰相對於地球坐標系的角度值 float sp = sinf(pitch); float sr = sinf(roll);//roll 表示橫滾相對於地球坐標系的角度值 float cr = cosf(roll); float sy = sinf(yaw);//yaw 表示偏航相對於地球坐標的角度值 float cy = cosf(yaw); a.x = cp * cy; a.y = (sr * sp * cy) - (cr * sy); a.z = (cr * sp * cy) + (sr * sy); b.x = cp * sy; b.y = (sr * sp * sy) + (cr * cy); b.z = (cr * sp * sy) - (sr * cy); c.x = -sp; c.y = sr * cp; c.z = cr * cp;}

其中a，b，c為類定義的私有變數---向量。通過不同的旋轉順序可以得到不同的旋轉矩陣，如果從地球坐標繫到體坐標系,按照 Z->X->Y 軸的順序旋轉可以得到from_euler312函數，這裡就沒做具體講解。

問題：反過來也就可以通過方向餘弦矩陣來求出旋轉角

函數 to_euler 式通過上面的 3 個公式求出對應的角度的

// calculate euler angles from a rotation matrix// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdftemplate <typename T>void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw) const{ if (pitch != NULL) { *pitch = -safe_asin(c.x); } if (roll != NULL) { *roll = atan2f(c.y, c.z); } if (yaw != NULL) { *yaw = atan2f(b.x, a.x); }}

地理坐標系中向量的每個分量等於相對應的旋轉矩陣的行與機體坐標向量的點積。計算旋轉矩陣需要9個乘法和6個加法運算。方程3是方程1的複述，用乘法展開向量和矩陣的元素。所以如果知道機體坐標向量，即可得地理坐標向量的大小：

需要注意的是，矩陣R不一定是對稱的。R矩陣的三列對應於機體坐標的三個軸向量到地理坐標的變換。R矩陣的三行則對應於地理坐標三個軸向量到機體坐標的變換。該R矩陣描述了所有機體相對於地球方向的信息。R矩陣也稱為方向餘弦矩陣，因為每個分量都是機體坐標軸與地理坐標軸夾角的餘弦，通過看推理餘弦矩陣部分可以看出來。

矩陣的轉置，特別在旋轉矩陣中，表示為
$R^{T}$

，通過交換行和列得到。一般來說，一個方形矩陣的逆矩陣如果存在的話，表示為

R^{? 1}

。矩陣的逆乘以矩陣得到的是單位矩陣。（單位矩陣就是對角線上元素為1,其餘為0,單位矩陣乘以任何矩陣得到它本身），對於旋轉矩陣來說，逆就等於它的轉置。

之所以旋轉矩陣逆就等於它的轉置考慮到了對稱性的情況。旋轉矩陣的元素都是機體軸與地理坐標軸之間的餘弦值，相反的情況就相當於交換地理坐標和機體坐標的角色，也就是說交換行與列，這跟轉置是一樣的。實際上這又和正交條件達成一致：

正交矩陣每一列都是單位矩陣，並且兩兩正交。最簡單的正交矩陣就是單位陣。

正交矩陣的逆（inverse）等於正交矩陣的轉置（transpose）。同時可以推論出正交矩陣的行列式的值肯定為正負1的。

正交矩陣滿足很多矩陣性質，比如可以相似於對角矩陣等等。

如下：

這個方程用來證明矩陣的逆的矩陣的轉置。

旋轉矩陣的一個非常有用的特性是，我們可以組成旋轉。

這裡特別需要小心運算順序，因為它的效果是完全不一樣的。

另外這裡還有一些有用的特性，如：

下面介紹一下點乘和叉乘。

這裡有兩個DCM計算裡邊用到的向量運算——點乘和叉乘。點乘是表量運算，A向量作為行向量，B向量作為列向量。

可以證明向量的點乘等於兩個向量的長度乘以它們角度的餘弦值。

所以向量的點乘是對稱的。A

?

B = B

?

而兩個向量的叉乘是一個向量。它的元素是這樣計算的：

從物理意義上來分析：

所以叉乘是反對稱的，A

\times

B = - B

\times

方向餘弦矩陣的更新

下面到了DCM演算法的核心部分——由陀螺儀計算方向餘弦矩陣。

核心概念：非線性微分方程——方向餘弦的變化速率與陀螺儀之間的關係。我們的目標是計算方向餘弦而不是任何違反方程非線性的近似解。目前，我們假設陀螺儀信號沒有錯誤。稍後我們將解決陀螺儀漂移問題。不像機械陀螺，我們不能通過簡單地積分陀螺儀信號得到角度。一個有名的運動學公式，關於旋轉向量的變化率和它的旋轉之間的關係：

下面解釋一下這個公式，如下圖所示，r為任意的旋轉向量，t時刻的坐標為r(t)。時間間隔dt後：r = r (t) , r』= r (t+dt) and dr = r』 – r。dt時間後向量r繞著與單位向量u同向的軸旋轉了d
$θ$

，停到了向量r"的位置。其中u垂直與旋轉的機身，因此u正交於r與r"，圖中顯示了u與u"，它們與r和r『的叉乘結果方向相同。故有u = (r x r』) / |r x r』| = (r x r』) / (|r|| r』|sin(dθ)) = (r x r』) / (|r|2 sin(dθ))由於旋轉並不改變向量的長度，因此有| r』| = |r|。向量r的線速度可以表示如下：v = dr / dt = ( r』 – r) / dtv = w x r故可以得出上面的公式。

有一個需要注意的地方：

微分方程是非線性的。旋轉向量輸入是與我們要進行積分的變數進行叉乘。因此，任何線性的方法都只是一種近似。

兩個向量都應該在同一個坐標系中測量。

因為叉乘是不對稱的，所以我們需要保存結果，然後改變它的方向。

如果知道了初始狀態和旋轉向量的時間，我們可以通過方程11的數值積分來跟蹤旋轉向量。

將方程13用於R矩陣的行或列中，可看作成旋轉向量。這裡遇到的第一個問題是，我們要跟蹤的向量和旋轉向量不是在同一坐標系做測量的。理想情況下，我們都是在地理坐標軸中跟蹤機體坐標軸，但是陀螺儀是在機體坐標中測量的。一個簡單的方法是通過對稱性解決，地理坐標在機體坐標中旋轉和機體坐標在地理坐標中旋轉是相反的，所以只要改變陀螺儀的符號就好了，更加方便的方法是，交換叉乘的順序就好了。

這裡的向量代表的是方程1中R矩陣的行。下面的問題是怎麼實施方程14，回歸到方程14的微分方程形式：

由這裡可知方向餘弦矩陣R的行都是通過

r_{e a r t h} (t) \times d θ (t)

積分得到的。所以根據陀螺儀的角度值,來計算當前機體的姿態 DCM矩陣，其使用的方法是：機體坐標的每個軸的向量與 g(陀螺儀改變的角度向量)求叉積，這裡求的是角度改變後,姿態在各個方向上的變化量，所以最後使用了矩陣的加法。源碼matrix3.cpp中的函數體現如下：

// apply an additional rotation from a body frame gyro vector// to a rotation matrix.template <typename T>void Matrix3<T>::rotate(const Vector3<T> &g){ Matrix3<T> temp_matrix; temp_matrix.a.x = a.y * g.z - a.z * g.y; temp_matrix.a.y = a.z * g.x - a.x * g.z; temp_matrix.a.z = a.x * g.y - a.y * g.x; temp_matrix.b.x = b.y * g.z - b.z * g.y; temp_matrix.b.y = b.z * g.x - b.x * g.z; temp_matrix.b.z = b.x * g.y - b.y * g.x; temp_matrix.c.x = c.y * g.z - c.z * g.y; temp_matrix.c.y = c.z * g.x - c.x * g.z; temp_matrix.c.z = c.x * g.y - c.y * g.x; (*this) += temp_matrix;}

還有一件事需要做，陀螺儀漂移將在後面進行。我們需要通過比例積分補償反饋控制器來添加旋轉速率校準到陀螺儀測量的數據上，以此產生最優的角速率估計。

基本上，我們的GPS和加速度計的參考向量被用來計算旋轉誤差，並通過反饋控制器輸入計算，然後更新原有計算。

我們可以把方程15轉化為矩陣的形式，這裡推導有點複雜，可以了解下矢量陣或者summer的文章，如下：

方程17就是從陀螺儀信號更新方向餘弦矩陣，其對角線上的1就為方程15的第一個條目，其餘的為第二個條目，這種方法是用矩陣的乘法實現的，它包含27個乘法和18個加法。正好適合dsPIC30F4011，因為它支持矩陣乘法運算，如果晶元不支持，也可以用方程15的積分形式實現。故在apm里採用的是積分形式累加的。矩陣更新的源碼體現如下：

// update the DCM matrix using only the gyrosvoidAP_AHRS_DCM::matrix_update(float _G_Dt){ // note that we do not include the P terms in _omega. This is // because the spin_rate is calculated from _omega.length(), // and including the P terms would give positive feedback into // the _P_gain() calculation, which can lead to a very large P // value _omega.zero(); // average across first two healthy gyros. This reduces noise on // systems with more than one gyro. We don"t use the 3rd gyro // unless another is unhealthy as 3rd gyro on PH2 has a lot more // noise uint8_t healthy_count = 0; Vector3f delta_angle; for (uint8_t i=0; i<_ins.get_gyro_count(); i++) { if (_ins.get_gyro_health(i) && healthy_count < 2) { Vector3f dangle; if (_ins.get_delta_angle(i, dangle)) { healthy_count++; delta_angle += dangle; } } } if (healthy_count > 1) { delta_angle /= healthy_count; //獲取角度變化量 } if (_G_Dt > 0) { _omega = delta_angle / _G_Dt; _omega += _omega_I; _dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * _G_Dt); //將角度變化量與旋轉向量進行叉乘，然後累加 }}

再歸一化重整

由於上述的公式是在dt非常小的情況下才誤差比較小，這種數字誤差將驅使方程4的正交條件逐漸不滿足，導致坐標系的兩個軸不再能描述剛體。值得幸運的是，數字誤差累加的很慢，我們完全可以提前採取辦法解決它。我們將這種解決辦法稱為歸一化，我們設計了幾種方法，模擬實現都可行，最後選擇了一種最優的方法。

首先計算矩陣列向量X、Y的點乘，理論上應該等於0，所以它的結果可以看出向量偏了多少。

將這個誤差平分到X、Y向量：

可以將方程19代人方程18中，驗證正交性誤差大大減少了。記住R矩陣的行與列的範數為1，將誤差平分給兩個軸比只分個一個產生較低的殘餘誤差。

下一步就是調整Z列向量正交於X和Y。這個很簡單，只要進行叉乘就可以了：

最後一步為，確保R矩陣的各列向量的模為1，一種方法可以通過平方根來求，但是這裡有一種更加簡單的辦法，考慮到這個模不會與1有太大差別，這裡可以使用泰勒展開。

方程21做的事情就是調整各列向量的模為1。展開為3減去向量模的平方，乘以1/2，再乘以這個向量。所以計算更加簡單。源碼實現如下：

/*************************************************- Direction Cosine Matrix IMU: Theory- William Premerlani and Paul Bizard *- Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5- to approximations rather than identities. In effect, the axes in the two frames of reference no- longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a- simple matter to stay ahead of it.- We call the process of enforcing the orthogonality conditions òrenormalizationó. */voidAP_AHRS_DCM::normalize(void){ float error; Vector3f t0, t1, t2; error = _dcm_matrix.a * _dcm_matrix.b; // eq.18 t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error)); // eq.19 t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error)); // eq.19 t2 = t0 % t1; // c= a x b // eq.20 if (!renorm(t0, _dcm_matrix.a) || !renorm(t1, _dcm_matrix.b) || !renorm(t2, _dcm_matrix.c)) { // Our solution is blowing up and we will force back // to last euler angles _last_failure_ms = AP_HAL::millis(); AP_AHRS_DCM::reset(true); }}// renormalise one vector component of the DCM matrix// this will return false if renormalization failsboolAP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result){ float renorm_val; // numerical errors will slowly build up over time in DCM, // causing inaccuracies. We can keep ahead of those errors // using the renormalization technique from the DCM IMU paper // (see equations 18 to 21). // For APM we don"t bother with the taylor expansion // optimisation from the paper as on our 2560 CPU the cost of // the sqrt() is 44 microseconds, and the small time saving of // the taylor expansion is not worth the potential of // additional error buildup. // Note that we can get significant renormalisation values // when we have a larger delta_t due to a glitch eleswhere in // APM, such as a I2c timeout or a set of EEPROM writes. While // we would like to avoid these if possible, if it does happen // we don"t want to compound the error by making DCM less // accurate. renorm_val = 1.0f / a.length(); //這裡並沒有使用泰勒展開，考慮的節省的時間不多 // keep the average for reporting _renorm_val_sum += renorm_val; _renorm_val_count++; if (!(renorm_val < 2.0f && renorm_val > 0.5f)) { // this is larger than it should get - log it as a warning if (!(renorm_val < 1.0e6f && renorm_val > 1.0e-6f)) { // we are getting values which are way out of // range, we will reset the matrix and hope we // can recover our attitude using drift // correction before we hit the ground! //Serial.printf("ERROR: DCM renormalisation error. renorm_val=%f ", // renorm_val); return false; } } result = a * renorm_val; return true;}

EKF設計與實現

資料搜集

pixhawk ekf

Matlab模擬EKF

Learning the Extended Kalman Filter

An application of the extended Kalman filter to the attitude control of a quadrotor

UAV Linear and Nonlinear Estimation Using Extended Kalman Filter/pdf

Performance analysis of a Kalman Filter based attitude estimator for a Quad Rotor UAV/pdf

Stabilization and Altitude Control of an Indoor Low-Cost Quadrotor: Design and Experimental Results/pdf

APM的EKF源碼流分析：ArduCopter.cpp里fast_loop函數里的姿態更新部分如下：

void AP_AHRS_NavEKF::update(void){ update_DCM(); update_EKF1(); update_EKF2();#if CONFIG_HAL_BOARD == HAL_BOARD_SITL update_SITL();#endif}

這裡看出姿態估算使用了DCM和EKF演算法，而EKF由分為兩種，第一種為默認推薦的穩定版代碼飛行的，第二種為git上master的，是另外一種形式的升級版EKF，這種EKF可以通過一個移動的平台起飛。下面以master版本分析，具體解釋如下：

void AP_AHRS_NavEKF::update_EKF2(void) //master版（即開發版）實驗表明，最後是由這個函數生成的姿態角{ if (!ekf2_started) { // wait 1 second for DCM to output a valid tilt error estimate if (start_time_ms == 0) { start_time_ms = AP_HAL::millis(); } if (AP_HAL::millis() - start_time_ms > startup_delay_ms) { ekf2_started = EKF2.InitialiseFilter(); //使用了DCM的_error_rp = 0.8f * _error_rp + 0.2f * best_error;，來源drift_correction，所以EKF依賴於DCM } } if (ekf2_started) { EKF2.UpdateFilter(); if (active_EKF_type() == EKF_TYPE2) { Vector3f eulers; EKF2.getRotationBodyToNED(_dcm_matrix); ////這裡的矩陣重新賦值了DCM矩陣，即姿態 EKF2.getEulerAngles(-1,eulers); roll = eulers.x; //生成roll pitch = eulers.y; yaw = eulers.z; update_cd_values(); //生成roll_sensor update_trig(); //生成_cos_roll // keep _gyro_bias for get_gyro_drift() EKF2.getGyroBias(-1,_gyro_bias); _gyro_bias = -_gyro_bias; // calculate corrected gryo estimate for get_gyro() _gyro_estimate.zero(); uint8_t healthy_count = 0; for (uint8_t i=0; i<_ins.get_gyro_count(); i++) { if (_ins.get_gyro_health(i) && healthy_count < 2) { _gyro_estimate += _ins.get_gyro(i); healthy_count++; } } if (healthy_count > 1) { _gyro_estimate /= healthy_count; } _gyro_estimate += _gyro_bias; float abias; EKF2.getAccelZBias(-1,abias); // This EKF uses the primary IMU // Eventually we will run a separate instance of the EKF for each IMU and do the selection and blending of EKF outputs upstream // update _accel_ef_ekf for (uint8_t i=0; i<_ins.get_accel_count(); i++) { Vector3f accel = _ins.get_accel(i); if (i==_ins.get_primary_accel()) { accel.z -= abias; } if (_ins.get_accel_health(i)) { _accel_ef_ekf[i] = _dcm_matrix * accel; } } _accel_ef_ekf_blended = _accel_ef_ekf[_ins.get_primary_accel()]; } }}

概要：在這個函數里有一個active_EKF_type()函數，通過分析可知它的值為EKF_TYPE2，故使用的是EKF2演算法。具體分析如下

AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const{ EKF_TYPE ret = EKF_TYPE_NONE;/* enum EKF_TYPE {EKF_TYPE_NONE=0, EKF_TYPE1=1, EKF_TYPE2=2#if CONFIG_HAL_BOARD == HAL_BOARD_SITL ,EKF_TYPE_SITL=10#endif };*/ switch (ekf_type()) { case 0: return EKF_TYPE_NONE; case 1: { // do we have an EKF yet? if (!ekf1_started) { return EKF_TYPE_NONE; } if (always_use_EKF()) {/* bool always_use_EKF() const { return _ekf_flags & FLAG_ALWAYS_USE_EKF; //下面分析這兩個數據,結果為0 }// constructorAP_AHRS_NavEKF::AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, RangeFinder &rng, NavEKF &_EKF1, NavEKF2 &_EKF2, Flags flags) : AP_AHRS_DCM(ins, baro, gps), EKF1(_EKF1), EKF2(_EKF2), _ekf_flags(flags){ _dcm_matrix.identity();}enum Flags { FLAG_NONE = 0, FLAG_ALWAYS_USE_EKF = 0x1, //FLAG_ALWAYS_USE_EKF = 0x1 }; // Constructor AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, RangeFinder &rng, NavEKF &_EKF1, NavEKF2 &_EKF2, Flags flags = FLAG_NONE); //所以_ekf_flags = FLAG_NONE = 0*/ uint8_t ekf_faults; EKF1.getFilterFaults(ekf_faults); if (ekf_faults == 0) { ret = EKF_TYPE1; } } else if (EKF1.healthy()) { ret = EKF_TYPE1; } break; } case 2: { //由下面的函數分析可知，應該會跳到這裡 // do we have an EKF2 yet? if (!ekf2_started) { return EKF_TYPE_NONE; } if (always_use_EKF()) { uint8_t ekf2_faults; EKF2.getFilterFaults(-1,ekf2_faults); if (ekf2_faults == 0) { ret = EKF_TYPE2; } } else if (EKF2.healthy()) { ret = EKF_TYPE2; //最終的返回結果 } break; }#if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKF_TYPE_SITL: ret = EKF_TYPE_SITL; break;#endif } /* fixed wing and rover when in fly_forward mode will fall back to DCM if the EKF doesn"t have GPS. This is the safest option as DCM is very robust */ if (ret != EKF_TYPE_NONE && (_vehicle_class == AHRS_VEHICLE_FIXED_WING || _vehicle_class == AHRS_VEHICLE_GROUND) && _flags.fly_forward) { nav_filter_status filt_state; if (ret == EKF_TYPE1) { EKF1.getFilterStatus(filt_state);#if CONFIG_HAL_BOARD == HAL_BOARD_SITL } else if (ret == EKF_TYPE_SITL) { get_filter_status(filt_state);#endif } else { EKF2.getFilterStatus(-1,filt_state); } if (hal.util->get_soft_armed() && !filt_state.flags.using_gps && _gps.status() >= AP_GPS::GPS_OK_FIX_3D) { // if the EKF is not fusing GPS and we have a 3D lock, then // plane and rover would prefer to use the GPS position from // DCM. This is a safety net while some issues with the EKF // get sorted out return EKF_TYPE_NONE; } if (hal.util->get_soft_armed() && filt_state.flags.const_pos_mode) { return EKF_TYPE_NONE; } if (!filt_state.flags.attitude || !filt_state.flags.horiz_vel || !filt_state.flags.vert_vel || !filt_state.flags.horiz_pos_abs || !filt_state.flags.vert_pos) { return EKF_TYPE_NONE; } } return ret;}

其中又涉及到了ekf_type函數，可知返回值為2：

//canonicalise _ekf_type, forcing it to be 0, 1 or 2uint8_t AP_AHRS_NavEKF::ekf_type(void) const{ uint8_t type = _ekf_type; //由下面的分析，默認_ekf_type的值為2 if (always_use_EKF() && type == 0) { //如果總是使用EKF且默認type為0時，那麼type就強製為1 type = 1; }// bool always_use_EKF() const {// return _ekf_flags & FLAG_ALWAYS_USE_EKF; //位運算，結果為0// } // check for invalid type#if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (type > 2 && type != EKF_TYPE_SITL) { //檢查type是否有效，只有為SIL的時候才能大於2 type = 1; }#else if (type > 2) { type = 1; }#endif return type;}

_ekf_type的默認取值分析。

#if AP_AHRS_NAVEKF_AVAILABLE //_ekf_type的默認取值為2 // @Param: EKF_TYPE // @DisplayName: Use NavEKF Kalman filter for attitude and position estimation // @Description: This controls whether the NavEKF Kalman filter is used for attitude and position estimation and whether fallback to the DCM algorithm is allowed. Note that on copters "disabled" is not available, and will be the same as "enabled - no fallback" // @Values: 0:Disabled,1:Enabled,2:Enable EKF2 // @User: Advanced AP_GROUPINFO("EKF_TYPE", 14, AP_AHRS, _ekf_type, 2),#endif

在DCM演算法里，與ekf演算法一樣，最後都生成了roll, roll_sensor， _cos_roll。

// calculate the euler angles and DCM matrix which will be used for high level// navigation control. Apply trim such that a positive trim value results in a// positive vehicle rotation about that axis (ie a negative offset)voidAP_AHRS_DCM::euler_angles(void){ _body_dcm_matrix = _dcm_matrix; _body_dcm_matrix.rotateXYinv(_trim); _body_dcm_matrix.to_euler(&roll, &pitch, &yaw); update_cd_values();}/* update the centi-degree values */void AP_AHRS::update_cd_values(void){ roll_sensor = degrees(roll) * 100; pitch_sensor = degrees(pitch) * 100; yaw_sensor = degrees(yaw) * 100; if (yaw_sensor < 0) yaw_sensor += 36000;}// update trig values including _cos_roll, cos_pitchupdate_trig();

EKF1與EKF2的切換：這裡假設你自己在開發版代碼中想使用EKF1怎麼辦？直接進行參數配置或者在地面站修改EKF相關的參數即可，具體如下修改即可

源碼修改，全局搜索到如下地方，更改成下面的樣子。
AP_GROUPINFO_FLAGS("ENABLE", 0, NavEKF2, _enable, 0, AP_PARAM_FLAG_ENABLE), AP_GROUPINFO_FLAGS("ENABLE", 34, NavEKF, _enable, 1, AP_PARAM_FLAG_ENABLE), AP_GROUPINFO("EKF_TYPE", 14, AP_AHRS, _ekf_type, 1),
地面站也是要修改三個地方。

可以總結出各個參數都在各自的cpp文件中有默認值如AP_MotorsMulticopter.cpp中的AP_GROUPINFO("CURR_MAX", 12, AP_MotorsMulticopter, _batt_current_max, AP_MOTORS_CURR_MAX_DEFAULT),設置解鎖時電機的轉速。

姿態控制

預習材料：PID參數調節/串級PID/串級PID1

下面將先進行APM源碼自穩模式的PID數據流分析：在AC_AttitudeControl.cpp里

void AC_AttitudeControl::attitude_controller_run_quat(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_rads){ // Update euler attitude target and angular velocity target att_target_quat.to_euler(_att_target_euler_rad.x,_att_target_euler_rad.y,_att_target_euler_rad.z); //將四元數的目標姿態角裝換為歐拉角 _att_target_ang_vel_rads = att_target_ang_vel_rads; // Retrieve quaternion vehicle attitude // TODO add _ahrs.get_quaternion() Quaternion att_vehicle_quat; att_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned()); //獲得機體姿態角 // Compute attitude error (att_vehicle_quat.inverse()*att_target_quat).to_axis_angle(_att_error_rot_vec_rad); //計算角度誤差 // Compute the angular velocity target from the attitude error update_ang_vel_target_from_att_error(); //內環P控制，更新_ang_vel_target_rads外環控制值 // Add the angular velocity feedforward, rotated into vehicle frame Matrix3f Trv; get_rotation_reference_to_vehicle(Trv); _ang_vel_target_rads += Trv * _att_target_ang_vel_rads; //更新_ang_vel_target_rads外環控制值}

其中get_rotation_body_to_ned函數的原型為：

const Matrix3f &AP_AHRS_NavEKF::get_rotation_body_to_ned(void) const{ if (!active_EKF_type()) { //如果沒有使用EKF,則使用DCM演算法生成的矩陣 return AP_AHRS_DCM::get_rotation_body_to_ned(); } return _dcm_matrix; //否則使用EKF2生成的矩陣}// return rotation matrix representing rotaton from body to earth axesonst Matrix3f &get_rotation_body_to_ned(void) const { return _body_dcm_matrix;}

另為，內環P控制，輸出值_ang_vel_target_rads為外環PID目標控制值。

void AC_AttitudeControl::update_ang_vel_target_from_att_error(){ // Compute the roll angular velocity demand from the roll angle error if (_att_ctrl_use_accel_limit && _accel_roll_max > 0.0f) { _ang_vel_target_rads.x = sqrt_controller(_att_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS)); }else{ _ang_vel_target_rads.x = _p_angle_roll.kP() * _att_error_rot_vec_rad.x; } // Compute the pitch angular velocity demand from the roll angle error if (_att_ctrl_use_accel_limit && _accel_pitch_max > 0.0f) { _ang_vel_target_rads.y = sqrt_controller(_att_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS)); }else{ _ang_vel_target_rads.y = _p_angle_pitch.kP() * _att_error_rot_vec_rad.y; } // Compute the yaw angular velocity demand from the roll angle error if (_att_ctrl_use_accel_limit && _accel_yaw_max > 0.0f) { _ang_vel_target_rads.z = sqrt_controller(_att_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss()/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS)); }else{ _ang_vel_target_rads.z = _p_angle_yaw.kP() * _att_error_rot_vec_rad.z; } // Add feedforward term that attempts to ensure that the copter yaws about the reference // Z axis, rather than the vehicle body Z axis. // NOTE: This is a small-angle approximation. _ang_vel_target_rads.x += _att_error_rot_vec_rad.y * _ahrs.get_gyro().z; _ang_vel_target_rads.y += -_att_error_rot_vec_rad.x * _ahrs.get_gyro().z;}

外環PID控制並設置電機值。

void AC_AttitudeControl::rate_controller_run(){ _motors.set_roll(rate_bf_to_motor_roll(_ang_vel_target_rads.x)); _motors.set_pitch(rate_bf_to_motor_pitch(_ang_vel_target_rads.y)); _motors.set_yaw(rate_bf_to_motor_yaw(_ang_vel_target_rads.z));}

這個函數位於ArduCopter.cpp里的fast_loop函數里，它是放於內環控制之前的。如下：

// Main loop - 400hzvoid Copter::fast_loop(){ // IMU DCM Algorithm // -------------------- read_AHRS(); // run low level rate controllers that only require IMU data attitude_control.rate_controller_run(); //外環並賦值給電機#if FRAME_CONFIG == HELI_FRAME update_heli_control_dynamics();#endif //HELI_FRAME // send outputs to the motors library motors_output(); // Inertial Nav // -------------------- read_inertia(); // check if ekf has reset target heading check_ekf_yaw_reset(); // run the attitude controllers update_flight_mode(); //如果僅考慮自穩模式，內環控制在這個裡邊 // update home from EKF if necessary update_home_from_EKF(); // check if we"ve landed or crashed update_land_and_crash_detectors();#if MOUNT == ENABLED // camera mount"s fast update camera_mount.update_fast();#endif // log sensor health if (should_log(MASK_LOG_ANY)) { Log_Sensor_Health(); }}

看看rate_controller_run函數其中的rate_bf_to_motor_roll函數，這裡進行了外環控制。

float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_rads){ float current_rate_rads = _ahrs.get_gyro().x; float rate_error_rads = rate_target_rads - current_rate_rads; // For legacy reasons, we convert to centi-degrees before inputting to the PID _pid_rate_roll.set_input_filter_d(degrees(rate_error_rads)*100.0f); _pid_rate_roll.set_desired_rate(degrees(rate_target_rads)*100.0f); float integrator = _pid_rate_roll.get_integrator(); // Ensure that integrator can only be reduced if the output is saturated if (!_motors.limit.roll_pitch || ((integrator > 0 && rate_error_rads < 0) || (integrator < 0 && rate_error_rads > 0))) { integrator = _pid_rate_roll.get_i(); } // Compute output float output = _pid_rate_roll.get_p() + integrator + _pid_rate_roll.get_d(); // Constrain output return constrain_float(output, -AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX, AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX);}

串口、GCS及LOG調試

由於我們在姿態演算法測試的過程當中需要及時查看相關數據的變化情況或者波形圖，在APM里主要有幾種方法可以實現我們的目的。第一種為串口：這種方式不僅在姿態演算法測試而且在系統調試的過程當中都很起作用，假如你的Pixhawk突然啟動出錯，這個時候你就可以通過串口查看板子上電後rcS（輸出信息都使用echo）的啟動過程。另一方面，在APM的代碼里，有獨立的庫文件可以進行編譯學習，目的是可以讓你單獨快速的學習這個模塊，如下，而這個裡邊的輸出信息直接列印到了串口。

void setup(){ hal.console->println("OpticalFlow library test ver 1.6"); hal.scheduler->delay(1000); // flowSensor initialization optflow.init(); if (!optflow.healthy()) { hal.console->print("Failed to initialise PX4Flow "); } hal.scheduler->delay(1000);}void loop(){ hal.console->println("this only tests compilation succeeds"); hal.scheduler->delay(5000);}AP_HAL_MAIN

串口的連接方式如下：

第二種方式為GCS：即通過飛控與地面站建立通信，在地面站上顯示圖形界面來顯示波形。下面結合源碼簡單分析一下GCS：從Arducopter.cpp可以看出，與地面站通信的主要線程為：

SCHED_TASK(gcs_check_input, 400, 180), SCHED_TASK(gcs_send_heartbeat, 1, 110), SCHED_TASK(gcs_send_deferred, 50, 550), SCHED_TASK(gcs_data_stream_send, 50, 550),

但是有一個地方需要注意，在void Copter::init_ardupilot()函數中，有一個設置：

// Register the mavlink service callback. This will run // anytime there are more than 5ms remaining in a call to // hal.scheduler->delay. hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);

而這個函數為mavlink_delay_cb_static，追蹤進去copter.mavlink_delay_cb();，再進去就是：

/* * a delay() callback that processes MAVLink packets. We set this as the * callback in long running library initialisation routines to allow * MAVLink to process packets while waiting for the initialisation to * complete */void Copter::mavlink_delay_cb(){ static uint32_t last_1hz, last_50hz, last_5s; if (!gcs[0].initialised || in_mavlink_delay) return; in_mavlink_delay = true; uint32_t tnow = millis(); if (tnow - last_1hz > 1000) { last_1hz = tnow; gcs_send_heartbeat(); gcs_send_message(MSG_EXTENDED_STATUS1); } if (tnow - last_50hz > 20) { last_50hz = tnow; gcs_check_input(); gcs_data_stream_send(); gcs_send_deferred(); notify.update(); } if (tnow - last_5s > 5000) { last_5s = tnow; gcs_send_text(MAV_SEVERITY_INFO, "Initialising APM"); } check_usb_mux(); in_mavlink_delay = false;}

可知在這裡也進行了數據的發送，那豈不是與之前的線程衝突了？仔細查看hal.scheduler->register_delay_callback這個函數:

void PX4Scheduler::register_delay_callback(AP_HAL::Proc proc, uint16_t min_time_ms) { _delay_cb = proc; _min_delay_cb_ms = min_time_ms;}

而_delay_cb，_min_delay_cb_ms在delay函數里得到了調用：

void PX4Scheduler::delay(uint16_t ms){ if (in_timerprocess()) { ::printf("ERROR: delay() from timer process "); return; } perf_begin(_perf_delay); uint64_t start = AP_HAL::micros64(); while ((AP_HAL::micros64() - start)/1000 < ms && !_px4_thread_should_exit) { delay_microseconds_semaphore(1000); if (_min_delay_cb_ms <= ms) { if (_delay_cb) { _delay_cb(); } } } perf_end(_perf_delay); if (_px4_thread_should_exit) { exit(1); }}

因此可以知道這裡的意思就是，如果系統有延時過長（_min_delay_cb_ms <= ms）的情況，則會給gcs發送數據，這樣以防止與gcs通信中斷。

主要發送消息流函數gcs_data_stream_send

/* * send data streams in the given rate range on both links */void Copter::gcs_data_stream_send(void){ for (uint8_t i=0; i<num_gcs; i++) { if (gcs[i].initialised) { gcs[i].data_stream_send(); } }}

如果我們繼續往裡邊看，可以看到這裡分類進行輸出。

voidGCS_MAVLINK::data_stream_send(void){ if (waypoint_receiving) { // don"t interfere with mission transfer return; } if (!copter.in_mavlink_delay && !copter.motors.armed()) { handle_log_send(copter.DataFlash); } copter.gcs_out_of_time = false; if (_queued_parameter != NULL) { if (streamRates[STREAM_PARAMS].get() <= 0) { streamRates[STREAM_PARAMS].set(10); } if (stream_trigger(STREAM_PARAMS)) { send_message(MSG_NEXT_PARAM); } // don"t send anything else at the same time as parameters return; } if (copter.gcs_out_of_time) return; if (copter.in_mavlink_delay) { // don"t send any other stream types while in the delay callback return; } if (stream_trigger(STREAM_RAW_SENSORS)) { send_message(MSG_RAW_IMU1); send_message(MSG_RAW_IMU2); send_message(MSG_RAW_IMU3); } if (copter.gcs_out_of_time) return; if (stream_trigger(STREAM_EXTENDED_STATUS)) { send_message(MSG_EXTENDED_STATUS1); send_message(MSG_EXTENDED_STATUS2); send_message(MSG_CURRENT_WAYPOINT); send_message(MSG_GPS_RAW); send_message(MSG_NAV_CONTROLLER_OUTPUT); send_message(MSG_LIMITS_STATUS); } if (copter.gcs_out_of_time) return; if (stream_trigger(STREAM_POSITION)) { send_message(MSG_LOCATION); send_message(MSG_LOCAL_POSITION); } if (copter.gcs_out_of_time) return; if (stream_trigger(STREAM_RAW_CONTROLLER)) { send_message(MSG_SERVO_OUT); } if (copter.gcs_out_of_time) return; if (stream_trigger(STREAM_RC_CHANNELS)) { send_message(MSG_RADIO_OUT); send_message(MSG_RADIO_IN); } if (copter.gcs_out_of_time) return; if (stream_trigger(STREAM_EXTRA1)) { send_message(MSG_ATTITUDE); send_message(MSG_SIMSTATE); send_message(MSG_PID_TUNING); } if (copter.gcs_out_of_time) return; if (stream_trigger(STREAM_EXTRA2)) { send_message(MSG_VFR_HUD); } if (copter.gcs_out_of_time) return; if (stream_trigger(STREAM_EXTRA3)) { send_message(MSG_AHRS); send_message(MSG_HWSTATUS); send_message(MSG_SYSTEM_TIME); send_message(MSG_RANGEFINDER);#if AP_TERRAIN_AVAILABLE && AC_TERRAIN send_message(MSG_TERRAIN);#endif send_message(MSG_BATTERY2); send_message(MSG_MOUNT_STATUS); send_message(MSG_OPTICAL_FLOW); send_message(MSG_GIMBAL_REPORT); send_message(MSG_MAG_CAL_REPORT); send_message(MSG_MAG_CAL_PROGRESS); send_message(MSG_EKF_STATUS_REPORT); send_message(MSG_VIBRATION); send_message(MSG_RPM); }}

這麼多消息，它的管理方式為隊列。

// send a message using mavlink, handling message queueingvoid GCS_MAVLINK::send_message(enum ap_message id){ uint8_t i, nextid; // see if we can send the deferred messages, if any while (num_deferred_messages != 0) { if (!try_send_message(deferred_messages[next_deferred_message])) { break; } next_deferred_message++; if (next_deferred_message == MSG_RETRY_DEFERRED) { next_deferred_message = 0; } num_deferred_messages--; } if (id == MSG_RETRY_DEFERRED) { return; } // this message id might already be deferred for (i=0, nextid = next_deferred_message; i < num_deferred_messages; i++) { if (deferred_messages[nextid] == id) { // its already deferred, discard return; } nextid++; if (nextid == MSG_RETRY_DEFERRED) { nextid = 0; } } if (num_deferred_messages != 0 || !try_send_message(id)) { // can"t send it now, so defer it if (num_deferred_messages == MSG_RETRY_DEFERRED) { // the defer buffer is full, discard return; } nextid = next_deferred_message + num_deferred_messages; if (nextid >= MSG_RETRY_DEFERRED) { nextid -= MSG_RETRY_DEFERRED; } deferred_messages[nextid] = id; num_deferred_messages++; }}

注意這裡邊有個所謂的get_secondary_attitude函數，它是另外一種演算法的姿態信息，不是DCM就是EKF：

// return secondary attitude solution if available, as eulers in radiansbool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers){ switch (active_EKF_type()) { case EKF_TYPE_NONE: // EKF is secondary EKF1.getEulerAngles(eulers); //這個是EKF1演算法生成的姿態信息 return ekf1_started; case EKF_TYPE1: case EKF_TYPE2: default: // DCM is secondary eulers = _dcm_attitude; //這是DCM演算法生成的姿態信息，應該是返回這裡 return true; }}

消息由mav_finalize_message_chan_send定製，最後由comm_send_buffer發出～如果飛控出現問題如，gcssend_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Nav Checks");，是這個函數發出來的。

在apmplanner里查看波形如下：

第三種當然是通過SD卡咯，一般來說，當我們實際飛行完成後，可以將SD裡面的飛行數據進行分析，對於APM的固件，數據打包成bin格式的文件存儲於SD卡中，可以使用apmplanner(如上圖中的OpenLog)讀取，也可以通過MAVExplorer。實際操作很簡單，不進行分析。APM的源碼體現為：

// fifty_hz_logging_loop// should be run at 50hzvoid Copter::fifty_hz_logging_loop(){#if HIL_MODE != HIL_MODE_DISABLED // HIL for a copter needs very fast update of the servo values gcs_send_message(MSG_RADIO_OUT);#endif#if HIL_MODE == HIL_MODE_DISABLED if (should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); Log_Write_Rate(); if (should_log(MASK_LOG_PID)) { DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() ); DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() ); DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() ); DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() ); } } // log IMU data if we"re not already logging at the higher rate if (should_log(MASK_LOG_IMU) && !(should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW))) { DataFlash.Log_Write_IMU(ins); }#endif}

參考文獻

陀螺儀加速度計MPU6050