PX4的無人機飛控應用開發(fā)

1、PX4/PixHawk無人機飛控應用開發(fā)1、PX4/Pixhawk飛控軟件架構簡介PX4是目前最流行的開源飛控板之一。PX4的軟件系統(tǒng)實際上就是一個firmware，其核心OS為NuttX實時ARM系統(tǒng)。其固件同時附帶了一系列工具集、系統(tǒng)驅動/模塊與外圍軟件接口層，所有這些軟件（包括用戶自定義的飛控軟件）隨OS內核一起，統(tǒng)一編譯為固件形式，然后上傳到飛控板中，從而實現(xiàn)對飛控板的軟件配置。PX4配套的軟件架構主要分為4層。理解其軟件架構是開發(fā)用戶自定義飛控應用軟件的基礎。a) API層：這個好理解。b) 框架層：包含了操作基礎飛行控制的默認程序集（節(jié)點）c) 系統(tǒng)庫：包含了所有的系統(tǒng)庫和基本交通

2、控制的函數(shù)d) OS內核：提供硬件驅動程序、網絡、UAVCAN和故障安全系統(tǒng)上述是個面向PX4系統(tǒng)實現(xiàn)者的相對具體的軟件架構。實際上還有另外一種面向PX4自定義飛控應用開發(fā)者的高層軟件架構描述，相對抽象，但更簡單，就是整個PX4的軟件從整體上分為2層：a) PX4 flight stack：一系列自治無人機自動控制算法的集合b) PX4 Middleware：一系列針對無人機控制器、傳感器等物理設備的驅動及底層通信、調度等機制的集合PX4軟件架構中，最有意思的一點在于整個架構的抽象性（多態(tài)性）。即，為了最大限度保障飛控算法代碼的重用性，其將飛控邏輯與具體的底層控制器指令實現(xiàn)進行了解耦合。一套高

3、層飛控算法（如autopilot、GeoFence等）在不做顯著修改的情況下，能夠適用于固定翼、直升機、多旋翼等多種機型的控制場合，這時候就體現(xiàn)出PX4飛控的威力來了：在用戶程序寫好之后，如果需要替換無人機機架的話，僅需簡單的修改一下機架配置參數(shù)即可，高層的用戶自定義飛控應用幾乎無需修改。理解上述初衷至關重要。有很多搞自動化出身、沒太多軟件經驗的朋友傾向于直接使用底層控制協(xié)議來控制飛控板，但實際上PX4架構已經在更高的抽象層面上提供了更好的選擇，無論是代碼維護成本、開發(fā)效率、硬件兼容性都能顯著高于前者。很多支持前者方式的開發(fā)者的理由主要在于高層封裝機制效率較低，而飛控板性能不夠，容易給飛控板造

4、成較大的處理負載，但實際從個人感覺上來看，遵循PX4的軟件架構模式反倒更容易實現(xiàn)較高處理性能，不容易產生控制擁塞，提升無人機側系統(tǒng)的并發(fā)處理效率。2、PX4/Pixhawk飛行控制協(xié)議與邏輯Mavlink是目前最常見的無人機飛控協(xié)議之一。PX4對Mavlink協(xié)議提供了良好的原生支持。該協(xié)議既可以用于地面站(GCS)對無人機(UAV)的控制，也可用于UAV對GCS的信息反饋。其飛控場景一般是這樣的：a) 手工飛控：GCS -> (MavLink) -> UAVb) 信息采集：GCS <- (Mavlink) <- UAVc) 自治飛控：User App -> (M

5、avLink) -> UAV也就是說，如果你想實現(xiàn)地面站控制飛行，那么由你的地面站使用Mavlink協(xié)議，通過射頻信道(或 wifi etc.)給無人機發(fā)送控制指令就可以了。如果你想實現(xiàn)無人機自主飛行，那么就由你自己寫的應用（運行在無人機系統(tǒng)上）使用Mavlink協(xié)議給無人機發(fā)送本地的控制指令就可以了。然而，為實現(xiàn)飛控架構的靈活性，避免對底層實現(xiàn)細節(jié)的依賴，在PX4中，并不鼓勵開發(fā)者在自定義飛控程序中直接使用Mavlink，而是鼓勵開發(fā)者使用一種名為uORB(Micro Object Request Broker，微對象請求代理)的消息機制。其實uORB在概念上等同于posix里面的命名

6、管道(named pipe)，它本質上是一種進程間通信機制。由于PX4實際使用的是NuttX實時ARM系統(tǒng)，因此uORB實際上相當于是多個進程（驅動級模塊）打開同一個設備文件，多個進程（驅動級模塊）通過此文件節(jié)點進行數(shù)據(jù)交互和共享。在uORB機制中，交換的消息被稱之為topic，一個topic僅包含一種message類型（即數(shù)據(jù)結構）。每個進程（或驅動模塊）均可“訂閱”或“發(fā)布”多個topic，一個topic可以存在多個發(fā)布者，而且一個訂閱者可也訂閱多個topic。而正因為有了uORB機制的存在，上述飛控場景變成了：a) 手工飛控：GCS -> (MavLink) -> (uORB

7、 topic) -> UAVb) 信息采集：GCS <- (Mavlink) <- (uORB topic) <- UAVc) 自治飛控：User App -> (uORB topic) -> (MavLink) -> UAV有了以上背景基礎，便可以自寫飛控邏輯了，僅需在PX4源碼中，添加一個自定義module，然后使用uORB訂閱相關信息（如傳感器消息等），并發(fā)布相關控制信息（如飛行模式控制消息等）即可。具體的uORB API、uORB消息定義可參考PX4文檔與源碼，所有控制命令都在firmware代碼的msg里面，不再敷述。最后值得一提的是，在PX

8、4系統(tǒng)中，還提供了一個名為mavlink的專用module，源碼在firmware的src/modules/mavlink中，這貨與linux的控制臺命令工具集相當相似，其既可以作為ntt控制臺下的命令使用，又可作為系統(tǒng)模塊加載后臺運行。其所實現(xiàn)的功能包括：1）uORB消息解析，將uORB消息實際翻譯為具體的Mavlink底層指令，或反之。2）通過serial/射頻通信接口獲取或發(fā)送Mavlink消息，既考慮到了用戶自寫程序的開發(fā)模式，也適用于類似linux的腳本工具鏈開發(fā)模式，使用起來很靈活，有興趣的可以看看。PX4飛控中利用EKF估計姿態(tài)角代碼詳解PX4飛控中主要用EKF算法來估計飛行器三

9、軸姿態(tài)角，具體c文件在px4Firmwaresrcmodulesattitude_estimator_ekfcodegen目錄下· 具體原理· 程序詳解· 下一步1.具體原理EKF算法原理不再多講，具體可參見上一篇blog 這篇講EKF算法執(zhí)行過程，需要以下幾個關鍵式子：· 飛行器狀態(tài)矩陣： 這里表示三軸角速度，表示三軸角加速度，表示加速度在機體坐標系三軸分量，表示磁力計在機體坐標系三軸分量。· 測量矩陣 分別由三軸陀螺儀，加速度計，磁力計測得。· 狀態(tài)轉移矩陣：飛行器下一時刻狀態(tài)預測矩陣如下：其中W項均

10、為高斯噪聲， 為飛行器在姿態(tài)發(fā)生變化后，坐標系余旋變換矩陣，對該函數(shù)在處求一階偏導，可得到狀態(tài)轉移矩陣：此時可得到飛行器狀態(tài)的先驗估計：· 利用測量值修正先驗估計：這里測量矩陣H與狀態(tài)矩陣X為線性關系，故無需求偏導?？柭鲆妫籂顟B(tài)后驗估計：方差后驗估計：2.程序詳解整個EKF的代碼挺長的，大部分是矩陣運算，而且使用嵌套for循環(huán)來執(zhí)行的，所以讀起來比較費勁，但是要是移植到自己工程上的話必然離不開這一步，所以花了一個下午把各個細節(jié)理清楚，順便記錄分享。/* Include files */#include "rt_nonfinite.h"#include

11、 "attitudeKalmanfilter.h"#include "rdivide.h"#include "norm.h"#include "cross.h"#include "eye.h"#include "mrdivide.h"/* '輸入參數(shù)：updateVect3：用來記錄陀螺儀，加速度計，磁力計傳感器數(shù)值是否有效 z9 ：測量矩陣 x_aposteriori_k12：上一時刻狀態(tài)后驗估計矩陣，用來預測當前狀態(tài) P_aposteriori_k144：上一時刻

12、后驗估計方差 eulerAngles3 ：輸出歐拉角 Rot_matrix9 ：輸出余弦矩陣 x_aposteriori12 ：輸出狀態(tài)后驗估計矩陣 P_aposteriori144 ：輸出方差后驗估計矩陣'*/void attitudeKalmanfilter(const uint8_T updateVect3,real32_T dt, const real32_T z9, const real32_T x_aposteriori_k12, const real32_T P_aposteriori_k144, const real32_T q12, real32_T r9, real

13、32_T eulerAngles3, real32_T Rot_matrix9,real32_T x_aposteriori12, real32_T P_aposteriori144)/*以下這一堆變量用到的時候再解釋*/ real32_T wak3; real32_T O9; real_T dv09; real32_T a9; int32_T i; real32_T b_a9; real32_T x_n_b3; real32_T b_x_aposteriori_k3; real32_T z_n_b3; real32_T c_a3; real32_T d_a3; int32_T i0; rea

14、l32_T x_apriori12; real_T dv1144; real32_T A_lin144; static const int8_T iv036 = 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ; real32_T b_A_lin144; real32_T b_q144; real32_T c_A_lin144; real32_T d_A_lin144; real32_T e_A_lin144; int32_T i

15、1; real32_T P_apriori144; real32_T b_P_apriori108; static const int8_T iv1108 = 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0

16、, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 ; real32_T K_k108; real32_T fv081; static const int8_T iv2108 = 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0

17、, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 ; real32_T b_r81; real32_T fv181; real32_T f0; real32_T c_P_apriori36= 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

18、1, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 ; real32_T fv236; static const int8_T iv436 = 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ; real32_T c_r9; real32_T b_K_k36; real32_T d_P_apriori72; static const int8_T iv572 = 1, 0, 0, 0, 0, 0, 0,

19、 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0 ; real32_T c_K_k72; static const int8_T iv672 = 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0,

20、0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ; real32_T b_z6; static const int8_T iv772 = 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 ; static

21、 const int8_T iv872 = 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1 ; real32_T fv36; real32_T c_z6; /*開始計算*/ /*'wak為當前狀態(tài)三軸角加速度'*/ wak0 = x_aposteriori_k3; wak1 = x_aposteriori_k4; wak2 = x_aposte

22、riori_k5;· 1· 2· 3· 4· 5· 6· 7· 8· 9· 10· 11· 12· 13· 14· 15· 16· 17· 18· 19· 20· 21· 22· 23· 24· 25· 26· 27· 28· 29· 30· 31· 32· 33

23、83; 34· 35· 36· 37· 38· 39· 40· 41· 42· 43· 44· 45· 46· 47· 48· 49· 50· 51· 52· 53· 54· 55· 56· 57· 58· 59· 60· 61· 62· 63· 64· 65· 66·

24、 67· 68· 69· 70· 71· 72· 73· 74· 75· 76· 77· 78· 79· 80· 81· 82· 83· 84· 85· 86· 87· 88· 89· 90· 91· 92· 93· 94· 95· 96· 97· 98· 99· 10

25、0· 101· 102· 103· 104· 105· 106· 107· 108· 109· 110· 111· 112· 113· 114· 115· 116· 117· 118· 119· 120· 121· 122· 123· 124· 125· 126· 127· 128· 129· 13

26、0· 131· 132· 133· 134· 135· 136· 137· 138· 139· 140· 141· 142· 143· 144· 145· 146· 147· 148· 149· 150· 151· 152· 153· 154· 155· 156· 157· 158· 159· 16

27、0· 161· 162/* 歐拉角旋轉矩陣 O=0wzwywz0wxwywx0這里的O矩陣相當于A矩陣中的的轉置矩陣！ */ O0 = 0.0F; O1 = -x_aposteriori_k2; O2 = x_aposteriori_k1; O3 = x_aposteriori_k2; O4 = 0.0F; O5 = -x_aposteriori_k0; O6 = -x_aposteriori_k1; O7 = x_aposteriori_k0; O8 = 0.0F; /* 預測轉過一個小角度之后的重力向量三軸投影 */ /* a = 1, -delta

28、_z, delta_y; * delta_z, 1 , -delta_x; * -delta_y, delta_x, 1 ' */ eye(dv0); /dv0矩陣單位化 for (i = 0; i < 9; i+) ai = (real32_T)dv0i + Oi * dt; /* 預測轉過一個小角度之后的磁力向量三軸投影 */ eye(dv0); for (i = 0; i < 9; i+) b_ai = (real32_T)dv0i + Oi * dt; · 1· 2· 3· 4· 5· 6· 7&

29、#183; 8· 9· 10· 11· 12· 13· 14· 15· 16· 17· 18· 19· 20· 21· 22· 23· 24· 25· 26· 27· 28/*a=1zyz1xyx1其實就是這個大家都很眼熟的的余弦矩陣的轉置, 用來更新機體轉過一個角度之后的重力和磁力三軸投影，只不過兩次計算間隔時間很短，變化角度很小，因此忽略高階小量之后就變成了這個樣子。這里還少一個時間系數(shù)dt，

30、下面會補上。 cosycoszcosysinzsiny(sinxsinycosz)+(cosxsinz)(sinxsinysinz)+(cosxcosz)sinxcosy(cosxsinycosz)+(sinxsinz)(cosxsinysinz)+(sinxcosz)cosxcosy*/ x_n_b0 = x_aposteriori_k0; /角速度 x_n_b1 = x_aposteriori_k1; x_n_b2 = x_aposteriori_k2; b_x_aposteriori_k0 = x_aposteriori_k6; / 加速度 b_x_aposteriori_k1

31、 = x_aposteriori_k7; b_x_aposteriori_k2 = x_aposteriori_k8; z_n_b0 = x_aposteriori_k9; /磁力計 z_n_b1 = x_aposteriori_k10; z_n_b2 = x_aposteriori_k11; for (i = 0; i < 3; i+) c_ai = 0.0F; for (i0 = 0; i0 < 3; i0+) c_ai += ai + 3 * i0 * b_x_aposteriori_ki0; d_ai = 0.0F; for (i0 = 0; i0 < 3; i0+)

32、 d_ai += b_ai + 3 * i0 * z_n_bi0; x_apriorii = x_n_bi + dt * waki; for (i = 0; i < 3; i+) x_apriorii + 3 = waki; for (i = 0; i < 3; i+) x_apriorii + 6 = c_ai; for (i = 0; i < 3; i+) x_apriorii + 9 = d_ai; /得到狀態(tài)先驗估計 · 1· 2· 3· 4· 5· 6· 7· 8· 9

33、3; 10· 11· 12· 13· 14· 15· 16· 17· 18· 19· 20· 21· 22· 23· 24· 25· 26· 27· 28· 29· 30· 31· 32· 33· 34/* 根據(jù)上述矩陣運算，可以得到： c_a13=axayaza33從而：kra,kt31=c_a13Td_a13=mxmymza33從而：

34、krm,kt31=d_a13T其中axayaz和mxmymz分別對應ra,k和rm,k的轉置；得到狀態(tài)先驗估計: Xk+1121=x_apriori112T=x_n_b+wakdtwakc_ad_aT*/* '開始計算A矩陣'*/ b_eye(dv1); for (i = 0; i < 12; i+) for (i0 = 0; i0 < 3; i0+) A_lini0 + 12 * i = (real32_T)iv0i0 + 3 * i; /*1 2 3列*/ for (i0 = 0; i0 < 3; i0+) A_lin(i0 + 12 * i)

35、+ 3 = 0.0F; /*3 4 5列*/ /*6 7 8 列*/ A_lin6 = 0.0F; A_lin7 = x_aposteriori_k8; A_lin8 = -x_aposteriori_k7; A_lin18 = -x_aposteriori_k8; A_lin19 = 0.0F; A_lin20 = x_aposteriori_k6; A_lin30 = x_aposteriori_k7; A_lin31 = -x_aposteriori_k6; A_lin32 = 0.0F; for (i = 0; i < 3; i+) for (i0 = 0; i0 < 3;

36、 i0+) A_lin(i0 + 12 * (i + 3) + 6 = 0.0F; for (i = 0; i < 3; i+) for (i0 = 0; i0 < 3; i0+) A_lin(i0 + 12 * (i + 6) + 6 = Oi0 + 3 * i; for (i = 0; i < 3; i+) for (i0 = 0; i0 < 3; i0+) A_lin(i0 + 12 * (i + 9) + 6 = 0.0F; /*6 7 8 列*/ /*9 10 11 列*/ A_lin9 = 0.0F; A_lin10 = x_aposteriori_k11;

37、 A_lin11 = -x_aposteriori_k10; A_lin21 = -x_aposteriori_k11; A_lin22 = 0.0F; A_lin23 = x_aposteriori_k9; A_lin33 = x_aposteriori_k7; A_lin34 = -x_aposteriori_k9; A_lin35 = 0.0F; for (i = 0; i < 3; i+) for (i0 = 0; i0 < 3; i0+) A_lin(i0 + 12 * (i + 3) + 9 = 0.0F; for (i = 0; i < 3; i+) for (

38、i0 = 0; i0 < 3; i0+) A_lin(i0 + 12 * (i + 6) + 9 = 0.0F; for (i = 0; i < 3; i+) for (i0 = 0; i0 < 3; i0+) A_lin(i0 + 12 * (i + 9) + 9 = Oi0 + 3 * i; /*9 10 11 列*/· 1· 2· 3· 4· 5· 6· 7· 8· 9· 10· 11· 12· 13· 14· 15

39、83; 16· 17· 18· 19· 20· 21· 22· 23· 24· 25· 26· 27· 28· 29· 30· 31· 32· 33· 34· 35· 36· 37· 38· 39· 40· 41· 42· 43· 44· 45· 46· 47· 48·

40、 49· 50· 51· 52· 53· 54· 55· 56· 57· 58· 59· 60· 61· 62· 63· 64· 65· 66· 67· 68/* 根據(jù)上述矩陣運算，可以得到A_lin矩陣： A_lin1212=0I000000A10O0A200O其中各元素都是3*3的矩陣；I為單位矩陣，其中 A1=0azayaz0axayax0=ra,Tk同樣A2=0mzmy

41、mz0mxmymx0=rm,Tk*/ for (i = 0; i < 12; i+) for (i0 = 0; i0 < 12; i0+) b_A_lini0 + 12 * i = (real32_T)dv1i0 + 12 * i + A_lini0 + 12 * i *dt; /最終A_link,k的逆矩陣· 1· 2· 3· 4· 5得到： Alin,Tk=b_A_lin1212=I0000I0000I0000I+0I000000A10O0A200Odt/* 開始根據(jù)計算過程方差 */ 過

42、程噪聲方差b_q1212=q00000q10000q20000q3其中各元素都是3*3的矩陣； b_q0 = q0; b_q12 = 0.0F; b_q24 = 0.0F; b_q36 = 0.0F; b_q48 = 0.0F; b_q60 = 0.0F; b_q72 = 0.0F; b_q84 = 0.0F; b_q96 = 0.0F; b_q108 = 0.0F; b_q120 = 0.0F; b_q132 = 0.0F; b_q1 = 0.0F; b_q13 = q0; b_q25 = 0.0F; b_q37 = 0.0F; b_q49 = 0.0F; b_q61 = 0.0F; b_q

43、73 = 0.0F; b_q85 = 0.0F; b_q97 = 0.0F; b_q109 = 0.0F; b_q121 = 0.0F; b_q133 = 0.0F; b_q2 = 0.0F; b_q14 = 0.0F; b_q26 = q0; b_q38 = 0.0F; b_q50 = 0.0F; b_q62 = 0.0F; b_q74 = 0.0F; b_q86 = 0.0F; b_q98 = 0.0F; b_q110 = 0.0F; b_q122 = 0.0F; b_q134 = 0.0F; b_q3 = 0.0F; b_q15 = 0.0F; b_q27 = 0.0F; b_q39 =

44、 q1; b_q51 = 0.0F; b_q63 = 0.0F; b_q75 = 0.0F; b_q87 = 0.0F; b_q99 = 0.0F; b_q111 = 0.0F; b_q123 = 0.0F; b_q135 = 0.0F; b_q4 = 0.0F; b_q16 = 0.0F; b_q28 = 0.0F; b_q40 = 0.0F; b_q52 = q1; b_q64 = 0.0F; b_q76 = 0.0F; b_q88 = 0.0F; b_q100 = 0.0F; b_q112 = 0.0F; b_q124 = 0.0F; b_q136 = 0.0F; b_q5 = 0.0F

45、; b_q17 = 0.0F; b_q29 = 0.0F; b_q41 = 0.0F; b_q53 = 0.0F; b_q65 = q1; b_q77 = 0.0F; b_q89 = 0.0F; b_q101 = 0.0F; b_q113 = 0.0F; b_q125 = 0.0F; b_q137 = 0.0F; b_q6 = 0.0F; b_q18 = 0.0F; b_q30 = 0.0F; b_q42 = 0.0F; b_q54 = 0.0F; b_q66 = 0.0F; b_q78 = q2; b_q90 = 0.0F; b_q102 = 0.0F; b_q114 = 0.0F; b_q

46、126 = 0.0F; b_q138 = 0.0F; b_q7 = 0.0F; b_q19 = 0.0F; b_q31 = 0.0F; b_q43 = 0.0F; b_q55 = 0.0F; b_q67 = 0.0F; b_q79 = 0.0F; b_q91 = q2; b_q103 = 0.0F; b_q115 = 0.0F; b_q127 = 0.0F; b_q139 = 0.0F; b_q8 = 0.0F; b_q20 = 0.0F; b_q32 = 0.0F; b_q44 = 0.0F; b_q56 = 0.0F; b_q68 = 0.0F; b_q80 = 0.0F; b_q92 =

47、 0.0F; b_q104 = q2; b_q116 = 0.0F; b_q128 = 0.0F; b_q140 = 0.0F; b_q9 = 0.0F; b_q21 = 0.0F; b_q33 = 0.0F; b_q45 = 0.0F; b_q57 = 0.0F; b_q69 = 0.0F; b_q81 = 0.0F; b_q93 = 0.0F; b_q105 = 0.0F; b_q117 = q3; b_q129 = 0.0F; b_q141 = 0.0F; b_q10 = 0.0F; b_q22 = 0.0F; b_q34 = 0.0F; b_q46 = 0.0F; b_q58 = 0.

48、0F; b_q70 = 0.0F; b_q82 = 0.0F; b_q94 = 0.0F; b_q106 = 0.0F; b_q118 = 0.0F; b_q130 = q3; b_q142 = 0.0F; b_q11 = 0.0F; b_q23 = 0.0F; b_q35 = 0.0F; b_q47 = 0.0F; b_q59 = 0.0F; b_q71 = 0.0F; b_q83 = 0.0F; b_q95 = 0.0F; b_q107 = 0.0F; b_q119 = 0.0F; b_q131 = 0.0F; b_q143 = q3; for (i = 0; i < 12; i+)

49、 for (i0 = 0; i0 < 12; i0+) A_lini + 12 * i0 = 0.0F; for (i1 = 0; i1 < 12; i1+) A_lini + 12 * i0 += b_A_lini + 12 * i1 * P_aposteriori_ki1 + 12 *i0; c_A_lini + 12 * i0 = 0.0F; for (i1 = 0; i1 < 12; i1+) c_A_lini + 12 * i0 += b_A_lini + 12 * i1 * b_qi1 + 12 * i0; for (i0 = 0; i0 < 12; i0+

50、) d_A_lini + 12 * i0 = 0.0F; for (i1 = 0; i1 < 12; i1+) d_A_lini + 12 * i0 += A_lini + 12 * i1 * b_A_lini0 + 12 * i1; e_A_lini + 12 * i0 = 0.0F; for (i1 = 0; i1 < 12; i1+) e_A_lini + 12 * i0 += c_A_lini + 12 * i1 * b_A_lini0 + 12 * i1; · 1· 2· 3· 4· 5· 6· 7&#

51、183; 8· 9· 10· 11· 12· 13· 14· 15· 16· 17· 18· 19· 20· 21· 22· 23· 24· 25· 26· 27· 28· 29· 30· 31· 32· 33· 34· 35· 36· 37· 38· 39· 40· 41· 42· 43· 44· 45· 46· 47