PX4的無人機(jī)飛控應(yīng)用開發(fā)

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

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

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

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

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

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

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

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

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

10、為高斯噪聲， 為飛行器在姿態(tài)發(fā)生變化后，坐標(biāo)系余旋變換矩陣，對(duì)該函數(shù)在處求一階偏導(dǎo)，可得到狀態(tài)轉(zhuǎn)移矩陣：此時(shí)可得到飛行器狀態(tài)的先驗(yàn)估計(jì)：· 利用測(cè)量值修正先驗(yàn)估計(jì)：這里測(cè)量矩陣H與狀態(tài)矩陣X為線性關(guān)系，故無需求偏導(dǎo)?？柭鲆妫籂顟B(tài)后驗(yàn)估計(jì)：方差后驗(yàn)估計(jì)：2.程序詳解整個(gè)EKF的代碼挺長(zhǎng)的，大部分是矩陣運(yùn)算，而且使用嵌套for循環(huán)來執(zhí)行的，所以讀起來比較費(fèi)勁，但是要是移植到自己工程上的話必然離不開這一步，所以花了一個(gè)下午把各個(gè)細(xì)節(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"/* '輸入?yún)?shù)：updateVect3：用來記錄陀螺儀，加速度計(jì)，磁力計(jì)傳感器數(shù)值是否有效 z9 ：測(cè)量矩陣 x_aposteriori_k12：上一時(shí)刻狀態(tài)后驗(yàn)估計(jì)矩陣，用來預(yù)測(cè)當(dāng)前狀態(tài) P_aposteriori_k144：上一時(shí)刻

12、后驗(yàn)估計(jì)方差 eulerAngles3 ：輸出歐拉角 Rot_matrix9 ：輸出余弦矩陣 x_aposteriori12 ：輸出狀態(tài)后驗(yàn)估計(jì)矩陣 P_aposteriori144 ：輸出方差后驗(yàn)估計(jì)矩陣'*/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)/*以下這一堆變量用到的時(shí)候再解釋*/ 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; /*開始計(jì)算*/ /*'wak為當(dāng)前狀態(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/* 歐拉角旋轉(zhuǎn)矩陣 O=0wzwywz0wxwywx0這里的O矩陣相當(dāng)于A矩陣中的的轉(zhuǎn)置矩陣！ */ 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; /* 預(yù)測(cè)轉(zhuǎn)過一個(gè)小角度之后的重力向量三軸投影 */ /* 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; /* 預(yù)測(cè)轉(zhuǎn)過一個(gè)小角度之后的磁力向量三軸投影 */ 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í)就是這個(gè)大家都很眼熟的的余弦矩陣的轉(zhuǎn)置, 用來更新機(jī)體轉(zhuǎn)過一個(gè)角度之后的重力和磁力三軸投影，只不過兩次計(jì)算間隔時(shí)間很短，變化角度很小，因此忽略高階小量之后就變成了這個(gè)樣子。這里還少一個(gè)時(shí)間系數(shù)dt，

30、下面會(huì)補(bǔ)上。 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; /磁力計(jì) 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)先驗(yàn)估計(jì) · 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ù)上述矩陣運(yùn)算，可以得到： c_a13=axayaza33從而：kra,kt31=c_a13Td_a13=mxmymza33從而：

34、krm,kt31=d_a13T其中axayaz和mxmymz分別對(duì)應(yīng)ra,k和rm,k的轉(zhuǎn)置；得到狀態(tài)先驗(yàn)估計(jì): Xk+1121=x_apriori112T=x_n_b+wakdtwakc_ad_aT*/* '開始計(jì)算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ù)上述矩陣運(yùn)算，可以得到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ù)計(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