ardupilot(EKF)擴展卡爾曼濾波

1、ardupilot(EKF)擴展卡爾曼濾波識卡爾曼濾波器為了描述方便我從網(wǎng)上找了一張卡爾曼濾波器的5大公式的圖片。篇幅所限，下圖所示的是多維卡爾曼濾波器(因為EKF2是多維擴展卡爾曼濾波器，所以我們從多維說起)，為了跟好的理解卡爾曼濾波器可以百度一下，從一維開始?？柭鼮V波器一時間更新和狀態(tài)更新-時間更新-狀態(tài)更新瓦=PkHT(HP,HTRyA-=龍子瓦仇-這5個公式之外還有一個觀測模型，根據(jù)你實際的觀測量來確定，它的主要作用是根據(jù)實際情況來求觀測矩陣H。因為卡爾曼濾波器是線性濾波器，狀態(tài)轉(zhuǎn)移矩陣A和觀測矩陣H是確定的。在維基百科上狀態(tài)轉(zhuǎn)移矩陣用F表示。在ardupilotEKF2算法中，狀態(tài)

2、轉(zhuǎn)移矩陣也是用F表示的。下面是維基百科給出的線性卡爾曼濾波器的相關(guān)公式。OSB碩詢徒親陋牯計益=-BitUi預(yù)制帖黠協(xié)方差P恥T=FJViiaiF+Qi更新缺創(chuàng)新姻量預(yù)眾豈能創(chuàng)軒或預(yù)先披臺族差）協(xié)方差是苗卡哲辱增益更新逅簡tK態(tài)牯廿更新【啟室命尿測莖后戰(zhàn)臺辰差上述更新（后驗）估計協(xié)方差的公式對任何增益Kk都有效，有時稱為約瑟夫形式。為了獲得最佳卡爾曼增益，該公式進一步簡化為Pk|k=（I-KkHk）Pk|k-1，它在哪種形式下應(yīng)用最廣泛。但是，必須記住它僅對最小化殘差誤差的最佳增益有效。為了使用卡爾曼濾波器來估計僅給出一系列噪聲觀測過程的內(nèi)部狀態(tài)，必須根據(jù)卡爾曼濾波器的框架對過程進行建模，這意

3、味著指定一下矩陣：F葉壯態(tài)轉(zhuǎn)移模型：H的觀案模型；QfrJ過程噪聲的協(xié)方差；R心觀察燥聲的協(xié)方差；有時B応是控制輸人模里，對于每于時間步氏乩如下所述：卡爾晏濾遍器桂型假定在時間#處的真實狀態(tài)星根揺（ri）處的狀態(tài)演孌帀來的冷=kk-i+E$；W+w&明徑譏是應(yīng)用二先前狀畝j的狀態(tài)轉(zhuǎn)移模型：Bw是控制磬扎槿型，應(yīng)用于控制向量是煜設(shè)從零均值多Tti卜態(tài)分布中得出的過程瑾聲，X,有協(xié)方差Qa：wfc川（hQjth在時間妙現(xiàn)察；或測量）總$真實狀態(tài)的X*艱據(jù)制成ZfrHfrxfr+*哪里H.滉將真里狀態(tài)空間映射和觀塞空間的觀訓(xùn)廩型八是觀測噪聲假設(shè)為零均值高斯白嗥同協(xié)方差為臉：現(xiàn)幾氏小初始狀態(tài)和每個步驟

4、w十嚴(yán)小SJ的噪岸矢量都視恨定為殆互獨芯ObservedSuppliedbyuserHiddenTime=k-1zft-lr創(chuàng)回固弩A/hTime=kTime=k-blzk+ir窗卡爾曼返破盎的基礎(chǔ)模里正方形代表矩陣桶同表示務(wù)元正態(tài)分布（包括均ff和協(xié)方差距陣）未計算的值是Q向呈，在簡單的情況下各種走陣隧時間平變，因此下標(biāo)被洲除，但卡訊亙?yōu)V無器允許它門中的任何一于比變母于時間歩長。只要記住一點就行了，卡爾曼濾波器的作用就是輸入一些包含噪聲的數(shù)據(jù)，得到一些比較接近真是情況的數(shù)據(jù)。比如無人機所使用的陀螺儀和加速度計的讀值，他們的讀值都是包含噪聲的，比如明明真實的角速度是俯仰2/s，陀螺儀的讀值卻是

5、2.5/s。通過擴展卡爾曼之后的角速度值會變得更加接近2/s的真實值，有可能是2.1/s。二擴展卡爾曼濾波器因為卡爾曼濾波器針對的是線性系統(tǒng)，狀態(tài)轉(zhuǎn)移模型（說的白話一點就是知道上一時刻被估計量的值，通過狀態(tài)轉(zhuǎn)移模型的公式可以推算出當(dāng)前時刻被估計量的值）和觀測模型。注：有的資料顯示狀態(tài)模型中有，有的沒有，目前我也不清楚是為什么，有可能和被估計的對象有關(guān)。但看多了你就會發(fā)現(xiàn)不管網(wǎng)上給的公式有怎樣的不同，但總體的流程是一樣的，都是這5大步驟。我個人覺得維基百科給的公式較為標(biāo)準(zhǔn)。因為擴展卡爾曼濾波器（EKF，ExtendedKalmanfilter）的使用場景為非線性系統(tǒng)。所以上面兩公式改寫為下面所示

6、的樣子，我個人的理解是，因為是非線性系統(tǒng)，所以沒有固定的狀態(tài)轉(zhuǎn)移矩陣和觀測矩陣。到這兒為止卡爾曼濾波器到擴展卡爾曼濾波器的過度就完成了（多說一句，因為傳感器的數(shù)據(jù)采樣是有時間間隔的，算法的運行也是有間隔的，所以本文提到的KF和EKF都是離散型的）。下面是擴展卡爾曼濾波器的相關(guān)公式。f（岔忌一1E_1砍）耳依一1=耳耳一屮1魂+Qt孤=視一1）皿=皿耳I+RkKh=PgH；時說打=kki+K賊Hpt=（-.心HQ耳軒4離散時間預(yù)測和更新方程【編輯預(yù)測編tt預(yù)測狀態(tài)估計預(yù)測協(xié)方差怙計更新編輯創(chuàng)新或計量剩余創(chuàng)新（或殘丟）協(xié)方差接近臺優(yōu)的卡爾曼增益更新狀態(tài)估計更新的協(xié)方差估計有心的你一定發(fā)現(xiàn)了，擴展卡

7、爾曼濾波器的狀態(tài)轉(zhuǎn)移和觀測模型中，沒有了狀態(tài)轉(zhuǎn)移矩陣和觀測矩陣：，但預(yù)測和更新過程中，還是要用到和：九就我個人的了解，求F和的過程就是對非線性系統(tǒng)線性化。F和H的求法如下:看到這兩個求和的公式，是不是頭都大了?？戳税胩煲膊恢朗鞘裁匆馑?，光看公式也不知道，這兩個矩陣到底該怎么求。我一開始就是這個感覺，什么雅克比矩陣，什么一階偏導(dǎo)，聽得頭都大了（還是大學(xué)數(shù)學(xué)沒學(xué)好.）。實際上數(shù)學(xué)家最擅長的事情就是把簡單的問題復(fù)雜化，把對問題的描述抽象化。抽象帶來的問題就是，公式具有高度的概括性，反映了一系列具有相同特性事物的關(guān)系，簡單明了的說明了事物的本質(zhì)。但抽象化掩蓋了細節(jié)，使人理解起來什么十分困難。隨后我會

8、結(jié)合EKF2的代碼作出詳細的解釋，參照著具體的情況和公式，反復(fù)多看幾遍，你就會有一個比較清晰的了解。想要深入了解ardupilotEKF2的原理，一定要把擴展卡爾曼濾波器的這幾個公式，背的滾瓜爛熟，這樣在后續(xù)讀代碼的時候，就能迅速聯(lián)想起代碼所對應(yīng)的過程和公式，這一點很重要。在了解EKF2的初期，并不需要知道擴展卡爾曼濾波器的公式為什么是這樣，你只要知道EKF2的每個階段，用的公式是什么樣子的就可以了。事實上我在為EKF2添加觀測量并測試成功的兩年之后，才逐漸的知道EKF2每一步所使用的公式都是什么。以自身經(jīng)驗來講，我覺得對EKF2的了解，需要經(jīng)歷3個階段。第一階段、不知其然最開始，你想要了解a

9、rdupilot姿態(tài)解算的（ardupilot的核心代碼大致分為幾塊，姿態(tài)解算（準(zhǔn)確的說應(yīng)該是位姿解算）、姿態(tài)控制、位置控制、任務(wù)決策、路徑規(guī)劃等，其中位姿解算是重中之重，其他模塊都依賴于位姿解算模塊所提供的準(zhǔn)確的姿態(tài)、位置信息）相關(guān)的代碼，看了它類的名字AP_NavEKF2，你在網(wǎng)上一搜或問了同事、同學(xué)、老師，知道了ardupilot使用的是擴展卡爾曼濾波器。你在網(wǎng)上開始查資料，這時你查到的資料魚龍混雜，有的說的是擴展卡爾曼濾波器，有的實際上說的是卡爾曼濾波器。在查資料的過程中你漸漸的知道了四元數(shù)、歐拉角、旋轉(zhuǎn)矩陣、旋轉(zhuǎn)向量等名詞，但此時的你還是一頭霧水。隨著你看的資料越來越多，你逐漸的知道

10、了擴展卡爾曼濾波器的作用，他的大體過程，他的輸入輸出是什么。雖然這時你也許還不大清楚為啥網(wǎng)上查到的公式有的地方長得不大一樣，但你發(fā)現(xiàn)他們大部分是一樣的。雖然你還不知道和是怎么求得的，對了，你有可能查到的公式里狀態(tài)轉(zhuǎn)移和觀測矩陣是和（我當(dāng)時查到的就是這樣，這直接導(dǎo)致后面看代碼更加的頭霧水）。此時，你覺得你對EKF有了一定的了解，開始看代碼。你努力的通過代碼了解EKF2的程序流程，一開始是一些邏輯代碼，你看得有些吃力，但還可以經(jīng)過一番鏖戰(zhàn)，你終于看到了EKF2的核心代碼（在此期間，相同的代碼你有可能會看好幾遍，而且每一遍的理解都不一樣）。當(dāng)你試著找到那5大公式時，你崩潰了。狀態(tài)轉(zhuǎn)移矩陣在哪？觀測矩

11、陣在哪？是什么？又是什么？怎么還有和？公式里也沒有提到呀。你再往下看，看到了和，你的心情稍微平復(fù)了一些，終于看到兩個公式里提到的變量。但這也和理論中的公式相差十萬八千里。再往后看，你看到了函數(shù)SelectMagFusion，此時的你猜想，這個函數(shù)的作用應(yīng)該是修正偏航角（Yaw）。你覺得這個函數(shù)有可能是一個突破口，于是你在粗略的看了看這個函數(shù)之后，確定了你的想法。之后的你了解了磁羅盤數(shù)據(jù)的格式和代表的含義，然后又開始讀代碼，讀著讀著，又看到了一大堆的計算，就在你痛苦不看之時，Kfusion出現(xiàn)在你的眼前。此時的你隱約的感覺到這個函數(shù)應(yīng)該是修正最終的輸出量（偏航角）的地方。在經(jīng)過無數(shù)個日夜，看代碼

12、，樹立猜想，再看代碼，推翻猜想的循環(huán)后，你終于知道了這個函數(shù)里關(guān)鍵變量的作用，但此時的你還是不知道那一大坨的計算是干什么的，不過多天的思考，你知道只要找到輸入，在修正量一致的情況下，計算過程應(yīng)該是一致的，你沿著這個思路，修改了輸入，發(fā)現(xiàn)輸出確實發(fā)生了變化。經(jīng)過無數(shù)個日夜的煎熬，終于送了口氣。此時的你沿著這個方向，經(jīng)過多次的修改、測試，終于實現(xiàn)了自己想要的結(jié)果。得到預(yù)想結(jié)果的你，高興極了。既然修改成功了，目標(biāo)達到了，EKF2里又有一大坨計算，看著都讓人頭疼，這個事情也可以放放了。畢竟自己已經(jīng)是修改過EKF2，ardupilot核心代碼的人了。第二階段、知其然當(dāng)你經(jīng)歷過第一階段后，你就去了解其他方

13、面的知識了（也有可能是，你實在看不懂那一坨計算代碼是干什么的，拼命的看了一段時間，還是沒有任何進展，這時投入產(chǎn)出比變得很低很低，你覺得不如先去了解下其他方面的知識）。在學(xué)習(xí)其他知識的過程中，你還是時常想起EKF2那些你沒有弄明白的問題，你時常翻看EKF2相關(guān)的代碼、之前找到的EKF資料，也時不時的從網(wǎng)上再查看一些新的資料，還有那個matlab腳本文件（GenerateNavFilterEquations.m）。通過注釋你知道了，EKF2的代碼是從這個腳本中來的，你在第一階段中也看過這個腳本，但也沒有看出來，EKF2是怎么通過這個腳本來。也許你對matlab也不是那么熟悉（語法似乎和C+差不多,

14、都是變量、函數(shù)，但又感覺差的很多），在看腳本的時候，看得迷迷糊糊，痛苦的很。所有在你翻看了這些資料后，你又去了解其他的知識了。期間你知道了ROS、了解了人工智能，知道了python的作用和應(yīng)用場景，所以你花時間學(xué)習(xí)了python。這時你還是時常想著ardupilotEKF2的實現(xiàn)到底是個怎樣的過程。在你不斷的思考和查資料中，你似乎對EKF的了解又深了一點點。此時你再一次的看了GenerateNavFilterEquations.m，這時你發(fā)現(xiàn)，你似乎可以讀懂這個腳本了，這個腳本的語法和python是那么的相似，但其中有幾行還是不一樣，你通過百度和自己的實際操作，也逐漸的弄明白了。在看腳本的過程

15、中，你發(fā)現(xiàn)，腳本中的狀態(tài)轉(zhuǎn)移矩陣用的是，以某種運算通過求得了，這時你恍然大悟，EKF代碼中的是和狀態(tài)轉(zhuǎn)移矩陣相關(guān)的。你之前一直在找的，在代碼中壓根就不存在，所以你之前一直感覺EKF2和EKF理論不是那么一致。之后，你又找到了EKF2代碼中存在的、。對應(yīng)的找到了EKF理論中的哪些公式。又花了一兩周時間，弄明白了這些公式所代表的含義，將EKF2的代碼和公式終于對應(yīng)了起來。但此時你又發(fā)現(xiàn)EKF2所用的公式和維基百科中的有一些區(qū)別，和你找的其他資料也不大一樣，和秦永元老師寫的卡爾曼濾波與組合導(dǎo)航原理一書中所寫的公式也不一樣，不是多一點就是少一點。因為ardupilot的廣泛應(yīng)用，所以你的直覺告訴你，E

16、KF2所用的公式雖然和資料里的都不一樣，但事實證明它是可用的，那它這么用公式也是可以的。為了加深你的理解，將EKF2所使用的公式，對照著代碼寫了一遍。此時你已經(jīng)明白了EKF2所使用的公式有哪些，狀態(tài)向量、狀態(tài)轉(zhuǎn)移矩陣、觀測矩陣和協(xié)方差矩陣都是什么。你已經(jīng)知道了在有新的傳感器后，如何添加到EKF2中，使其提高系統(tǒng)對自身姿態(tài)、位置或速度的估計。進行怎樣的測試，來證明你的添加量是能用的。但你此時依然不清楚EKF2所用的公式為什么是那樣，換個公式行不行，EKF的哪些參數(shù)影響了它的結(jié)果，影響有多大？是如何影響的？第三階段、知其所以然在經(jīng)歷了一、二階段之后，你對EKF和其在ardupilot中的實現(xiàn)（EK

17、F2）有了一定的了解，但第二階段完成后，你還是有許多的疑問。要解決這些疑問你必須要對EKF有更深層次的了解，知道EKF哪些公式是怎么推導(dǎo)來的。這需要較深的數(shù)學(xué)基礎(chǔ)。第三階段注定是個硬骨頭，完成它需要很多理論知識，包括數(shù)學(xué)知識、EKF相關(guān)的知識、導(dǎo)航相關(guān)的知識、傳感器相關(guān)的知識。但你了解了這些知識，并用這些知識解答了EKF2為什么要選取那幾個公式，那幾個公式是不是最優(yōu)選擇的問題。在解決了這些問題后，你可以針對多旋翼用C/C+實現(xiàn)一個支持IMU、GPS和磁羅盤的位姿解算算法。如果這些事情你都做完了，恭喜你，你已經(jīng)成為EKF方面的大牛了。三、一睹EKF2芳容預(yù)測過程首先我們來確定EKF算法預(yù)測過程兩

18、個方程（預(yù)測狀態(tài)估計方程和預(yù)測協(xié)方差估計方程）中的函數(shù)和變量。也就是狀態(tài)向量Xk、函數(shù)f（xk-l,uk-l,wk-1）、狀態(tài)轉(zhuǎn)移矩陣F、協(xié)方差矩陣P、過程噪聲協(xié)方差矩陣Q和不知名的矩陣L。其中P和Q的初值是根據(jù)相關(guān)傳感器和被估計量的特性定出來的，具體為什么定的是代碼中寫的那些值，我目前也不清楚。定義狀態(tài)向量：Xk總共有24個狀態(tài)，分別是以旋轉(zhuǎn)矢量表示的三軸角度誤差、三軸速度（北東地坐標(biāo)系）、三軸位置（北東地坐標(biāo)系，以濾波器開始運行的點為坐標(biāo)原點）、陀螺儀三軸偏差、陀螺儀三軸比例因子、加速度計Z軸偏差、三軸地磁場（地磁場在北東地坐標(biāo)系下的三軸分量）、三軸磁偏量（磁羅盤和機體坐標(biāo)系沒有對齊而產(chǎn)生

19、的偏差，注意body_magfield表示的并不是磁羅盤測試出的三軸磁分量！）、兩軸風(fēng)速（北東地坐標(biāo)系下的北和東）。四元數(shù)并不在狀態(tài)向量中。/本代碼段在AP_NavEKF2_core.h中Vector28statesArray;structstate_elementsVector3fangErr;/0.2Vector3fvelocity;/3.5Vector3fposition;/6.8Vector3fgyro_bias;/9.11Vector3fgyro_scale;/12.14floataccel_zbias;/15Vector3fearth_magfield;/16.18Vector3f

20、body_magfield;/19.21Vector2fwind_vel;/22.23Quaternionquat;/24.27&stateStruct;定義協(xié)方差矩陣尸：在voidNavEKF2_core:CovarianceInit（）函數(shù)中。定義過程噪聲協(xié)方差矩陣，其中三軸角度誤差、三軸速度（北東地坐標(biāo)系）、三軸位置（北東地坐標(biāo)系）的噪聲是非加性的，在SG中，所以24維向量processNoise的前9個元素是0。在函數(shù)NavEKF2_core:CovariancePrediction（）中下面的一段代碼出自GenerateNavFilterEquations.m，它的作用是定義了,進而推導(dǎo)出了狀