ardupilot(EKF)擴(kuò)展卡爾曼濾波

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

19、的偏差，注意body_magfield表示的并不是磁羅盤(pán)測(cè)試出的三軸磁分量！）、兩軸風(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ù)中。定義過(guò)程噪聲協(xié)方差矩陣，其中三軸角度誤差、三軸速度（北東地坐標(biāo)系）、三軸位置（北東地坐標(biāo)系）的噪聲是非加性的，在SG中，所以24維向量processNoise的前9個(gè)元素是0。在函數(shù)NavEKF2_core:CovariancePrediction（）中下面的一段代碼出自GenerateNavFilterEquations.m，它的作用是定義了,進(jìn)而推導(dǎo)出了狀