傳感器融合 卡爾曼：1.使用激光雷達(dá)數(shù)據(jù)的基礎(chǔ)卡爾曼濾波器：
展開全文
卡爾曼濾波器是傳感器融合工程師用于自動駕駛汽車的工具。想象一下，你有一個(gè)雷達(dá)傳感器，告訴你另一輛車距離15米，一個(gè)激光傳感器說車輛距離20米。你如何協(xié)調(diào)這些傳感器測量？這就是卡爾曼濾波器的功能。卡爾曼濾波在自動駕駛汽車上的應(yīng)用十分廣泛，本文講述卡爾曼濾波算法，希望對你有所幫助。
卡爾曼濾波算法在控制領(lǐng)域有極廣泛的應(yīng)用，在發(fā)動機(jī)燃油噴射控制中，可以應(yīng)用擴(kuò)展的卡爾曼濾波理論研究瞬態(tài)工況下發(fā)動機(jī)循環(huán)進(jìn)氣量的最優(yōu)估計(jì)算法，在雷達(dá)中，人們感興趣的是跟蹤目標(biāo)，但目標(biāo)的位置、速度、加速度的測量值往往在任何時(shí)候都有噪聲。卡爾曼濾波利用目標(biāo)的動態(tài)信息,設(shè)法去掉噪聲的影響，得到一個(gè)關(guān)于目標(biāo)位置的好的估計(jì)。
為了以后更好的工程實(shí)踐應(yīng)用卡爾曼濾波算法，今天小編帶領(lǐng)著大家了解卡爾曼濾波算法的理論，及其在自動駕駛多傳感器融合算法中的應(yīng)用。ー 1 ー什么是卡爾曼濾波？
你可以在任何含有不確定信息的動態(tài)系統(tǒng)中使用卡爾曼濾波，對系統(tǒng)下一步的走向做出有根據(jù)的預(yù)測，即使伴隨著各種干擾，卡爾曼濾波總是能指出真實(shí)發(fā)生的情況。
在連續(xù)變化的系統(tǒng)中使用卡爾曼濾波是非常理想的，它具有占用內(nèi)存小的優(yōu)點(diǎn)（除了前一個(gè)狀態(tài)量外，不需要保留其它歷史數(shù)據(jù)），并且速度很快，很適合應(yīng)用于實(shí)時(shí)問題和嵌入式系統(tǒng)。
在Google上找到的大多數(shù)關(guān)于實(shí)現(xiàn)卡爾曼濾波的數(shù)學(xué)公式看起來有點(diǎn)晦澀難懂，這個(gè)狀況有點(diǎn)糟糕。實(shí)際上，如果以正確的方式看待它，卡爾曼濾波是非常簡單和容易理解的，下面我將用漂亮的圖片和色彩清晰的闡述它，你只需要懂一些基本的概率和矩陣的知識就可以了。
ー 2 ー我們能用卡爾曼濾波做什么？
用玩具舉例：你開發(fā)了一個(gè)可以在樹林里到處跑的小機(jī)器人，這個(gè)機(jī)器人需要知道它所在的確切位置才能導(dǎo)航。我們可以說機(jī)器人有一個(gè)狀態(tài)： ，表示位置和速度：
注意這個(gè)狀態(tài)只是關(guān)于這個(gè)系統(tǒng)基本屬性的一堆數(shù)字，它可以是任何其它的東西。在這個(gè)例子中是位置和速度，它也可以是一個(gè)容器中液體的總量，汽車發(fā)動機(jī)的溫度，用戶手指在觸摸板上的位置坐標(biāo)，或者任何你需要跟蹤的信號。這個(gè)機(jī)器人帶有GPS，精度大約為10米，還算不錯(cuò)，但是，它需要將自己的位置精確到10米以內(nèi)。樹林里有很多溝壑和懸崖，如果機(jī)器人走錯(cuò)了一步，就有可能掉下懸崖，所以只有GPS是不夠的。或許我們知道一些機(jī)器人如何運(yùn)動的信息：例如，機(jī)器人知道發(fā)送給電機(jī)的指令，知道自己是否在朝一個(gè)方向移動并且沒有人干預(yù)，在下一個(gè)狀態(tài)，機(jī)器人很可能朝著相同的方向移動。當(dāng)然，機(jī)器人對自己的運(yùn)動是一無所知的：它可能受到風(fēng)吹的影響，輪子方向偏了一點(diǎn)，或者遇到不平的地面而翻倒。所以，輪子轉(zhuǎn)過的長度并不能精確表示機(jī)器人實(shí)際行走的距離，預(yù)測也不是很完美。GPS 傳感器告訴了我們一些狀態(tài)信息，我們的預(yù)測告訴了我們機(jī)器人會怎樣運(yùn)動，但都只是間接的，并且伴隨著一些不確定和不準(zhǔn)確性。但是，如果使用所有對我們可用的信息，我們能得到一個(gè)比任何依據(jù)自身估計(jì)更好的結(jié)果嗎？回答當(dāng)然是YES，這就是卡爾曼濾波的用處。ー 3 ー卡爾曼濾波是如何看到你的問題的？
下面我們繼續(xù)以只有位置和速度這兩個(gè)狀態(tài)的簡單例子做解釋。我們并不知道實(shí)際的位置和速度，它們之間有很多種可能正確的組合，但其中一些的可能性要大于其它部分： 卡爾曼濾波假設(shè)兩個(gè)變量（位置和速度，在這個(gè)例子中）都是隨機(jī)的，并且服從高斯分布。每個(gè)變量都有一個(gè)均值μ，表示隨機(jī)分布的中心（最可能的狀態(tài)），以及方差，表示不確定性。在上圖中，位置和速度是不相關(guān)的，這意味著由其中一個(gè)變量的狀態(tài)無法推測出另一個(gè)變量可能的值。下面的例子更有趣：位置和速度是相關(guān)的，觀測特定位置的可能性取決于當(dāng)前的速度： 這種情況是有可能發(fā)生的，例如，我們基于舊的位置來估計(jì)新位置。如果速度過高，我們可能已經(jīng)移動很遠(yuǎn)了。如果緩慢移動，則距離不會很遠(yuǎn)。跟蹤這種關(guān)系是非常重要的，因?yàn)樗鼛Ыo我們更多的信息：其中一個(gè)測量值告訴了我們其它變量可能的值，這就是卡爾曼濾波的目的，盡可能地在包含不確定性的測量數(shù)據(jù)中提取更多信息！ ：這種相關(guān)性用協(xié)方差矩陣來表示，簡而言之，矩陣中的每個(gè)元素， 表示第 i 個(gè)和第 j 個(gè)狀態(tài)變量之間的相關(guān)度。（你可能已經(jīng)猜到協(xié)方差矩陣是一個(gè)對稱矩陣，這意味著可以任意交換 i 和 j）。協(xié)方差矩陣通常用來表示，其中的元素則表示為ー 4 ー使用矩陣來描述問題
我們基于高斯分布來建立狀態(tài)變量，所以在時(shí)刻 k 需要兩個(gè)信息：最佳估計(jì)
（即均值，其它地方常用 μ 表示），以及協(xié)方差矩陣。
（當(dāng)然，在這里我們只用到了位置和速度，實(shí)際上這個(gè)狀態(tài)可以包含多個(gè)變量，代表任何你想表示的信息）。接下來，我們需要根據(jù)當(dāng)前狀態(tài)（k-1 時(shí)刻）來預(yù)測下一狀態(tài)（k 時(shí)刻）。記住，我們并不知道對下一狀態(tài)的所有預(yù)測中哪個(gè)是“真實(shí)”的，但我們的預(yù)測函數(shù)并不在乎。它對所有的可能性進(jìn)行預(yù)測，并給出新的高斯分布。我們可以用矩陣來表示這個(gè)預(yù)測過程：
它將我們原始估計(jì)中的每個(gè)點(diǎn)都移動到了一個(gè)新的預(yù)測位置，如果原始估計(jì)是正確的話，這個(gè)新的預(yù)測位置就是系統(tǒng)下一步會移動到的位置。那我們又如何用矩陣來預(yù)測下一個(gè)時(shí)刻的位置和速度呢？下面用一個(gè)基本的運(yùn)動學(xué)公式來表示：現(xiàn)在，我們有了一個(gè)預(yù)測矩陣來表示下一時(shí)刻的狀態(tài)，但是，我們?nèi)匀徊恢涝趺锤聟f(xié)方差矩陣。此時(shí)，我們需要引入另一個(gè)公式，如果我們將分布中的每個(gè)點(diǎn)都乘以矩陣 A，它的協(xié)方差矩陣系會怎樣變化呢？很簡單，下面給出公式：結(jié)合方程（4）和（3）得到： ー 5 ー外部控制量我們并沒有捕捉到一切信息，可能存在外部因素會對系統(tǒng)進(jìn)行控制，帶來一些與系統(tǒng)自身狀態(tài)沒有相關(guān)性的改變。 以火車的運(yùn)動狀態(tài)模型為例，火車司機(jī)可能會操縱油門，讓火車加速。相同地，在我們機(jī)器人這個(gè)例子中，導(dǎo)航軟件可能會發(fā)出一個(gè)指令讓輪子轉(zhuǎn)向或者停止。如果知這些額外的信息，我們可以用一個(gè)向量來表示，將它加到我們的預(yù)測方程中做修正。假設(shè)由于油門的設(shè)置或控制命令，我們知道了期望的加速度，根據(jù)基本的運(yùn)動學(xué)方程可以得到：
以矩陣的形式表示就是： 稱為控制矩陣，稱為控制向量（對于沒有外部控制的簡單系統(tǒng)來說，這部分可以忽略）。讓我們再思考一下，如果我們的預(yù)測并不是100%準(zhǔn)確的，該怎么辦呢？ー 6 ー外部干擾如果這些狀態(tài)量是基于系統(tǒng)自身的屬性或者已知的外部控制作用來變化的，則不會出現(xiàn)什么問題。但是，如果存在未知的干擾呢？例如，假設(shè)我們跟蹤一個(gè)四旋翼飛行器，它可能會受到風(fēng)的干擾，如果我們跟蹤一個(gè)輪式機(jī)器人，輪子可能會打滑，或者路面上的小坡會讓它減速。這樣的話我們就不能繼續(xù)對這些狀態(tài)進(jìn)行跟蹤，如果沒有把這些外部干擾考慮在內(nèi)，我們的預(yù)測就會出現(xiàn)偏差。 在每次預(yù)測之后，我們可以添加一些新的不確定性來建立這種與“外界”（即我們沒有跟蹤的干擾）之間的不確定性模型： 原始估計(jì)中的每個(gè)狀態(tài)變量更新到新的狀態(tài)后，仍然服從高斯分布。我們可以說的每個(gè)狀態(tài)變量移動到了一個(gè)新的服從高斯分布的區(qū)域，協(xié)方差為
。換句話說就是，我們將這些沒有被跟蹤的干擾當(dāng)作是協(xié)方差的的噪聲來處理。這產(chǎn)生了具有不同協(xié)方差（但是具有相同的均值）的新的高斯分布。我們通過簡單地添加得到的擴(kuò)展的協(xié)方差，下面給出預(yù)測步驟的完整表達(dá)式：由上式可知，新的最優(yōu)估計(jì)是根據(jù)上一個(gè)最優(yōu)估計(jì)預(yù)測的道德，并加上已知外部控制量的修正。而新的不確定性由上一不確定性預(yù)測得到，并加上外部環(huán)境的干擾。好了，我們對系統(tǒng)可能的動向有了一個(gè)模糊的估計(jì)，用和來表示。如果結(jié)合傳感器的數(shù)據(jù)會怎樣呢？
ー 7 ー用測量值來修正估計(jì)值我們可能會有多個(gè)傳感器來測量系統(tǒng)當(dāng)前的狀態(tài)，哪個(gè)傳感器具體測量的是哪個(gè)狀態(tài)變量并不重要，也許一個(gè)是測量位置，一個(gè)是測量速度，每個(gè)傳感器間接地告訴了我們一些狀態(tài)信息。注意，傳感器讀取的數(shù)據(jù)的單位和尺度有可能與我們要跟蹤的狀態(tài)額單位和尺度不一樣，我們用矩陣來表示傳感器的數(shù)據(jù)。我們可以計(jì)算出傳感器讀數(shù)的分布，用之前的表示方法如下式所示： 卡爾曼濾波的一大優(yōu)點(diǎn)就是能處理傳感器噪聲，換句話說，我們的傳感器或多或少都有點(diǎn)不可靠，并且原始估計(jì)中的每個(gè)狀態(tài)可以和一定范圍內(nèi)的傳感器讀數(shù)對應(yīng)起來。
從測量到的傳感器數(shù)據(jù)中，我們大致能猜到系統(tǒng)當(dāng)前處于什么狀態(tài)。但是由于存在不確定性，某些狀態(tài)可能比我們得到的讀數(shù)更接近真實(shí)狀態(tài)。
我們將這種不確定性（例如：傳感器噪聲）用協(xié)方差表示，該分布的均值就是我們讀取到的傳感器數(shù)據(jù)，稱之為：傳感器噪 現(xiàn)在我們有了兩個(gè)高斯分布，一個(gè)是在預(yù)測值附近，一個(gè)是在傳感器讀數(shù)附近。我們必須在預(yù)測值（粉紅色）和傳感器測量值（綠色）之間找到最優(yōu)解。 那么，我們最有可能的狀態(tài)是什么呢？對于任何可能的度數(shù)有兩種情況：（1）傳感器的測量值；（2）由前一狀態(tài)得到的預(yù)測值。如果我們想知道這兩種情況都可能發(fā)生的概率，將這兩個(gè)高斯分布相乘就可以了。剩下的就是重疊部分了，這個(gè)重疊部分的均值就是兩個(gè)估計(jì)最可能的值，也就是給定的所有信息中的最優(yōu)估計(jì)。瞧！這個(gè)重疊的區(qū)域看起來像另一個(gè)高斯分布。如你所見，把兩個(gè)具有不同均值和方差的高斯分布相乘，你會得到一個(gè)新的具有獨(dú)立均值和方差的高斯分布！下面用公式講解。ー 8 ー融合高斯分布先以一維高斯分布來分析比較簡單點(diǎn)，具有方差 和 μ 的高斯曲線可以用下式表示：
如果把兩個(gè)服從高斯分布的函數(shù)相乘會得到什么呢？
將式（9）代入到式（10）中（注意重新歸一化，使總概率為1）可以得到： 將式（11）中的兩個(gè)式子相同的部分用 k 表示：
下面進(jìn)一步將式（12）和（13）寫成矩陣的形式，如果 Σ 表示高斯分布的協(xié)方差， 表示每個(gè)維度的均值，則：
矩陣稱為卡爾曼增益，下面將會用到。放松！我們快要完成了！
將所有公式結(jié)合起來我們有兩個(gè)高斯分布，預(yù)測部分，和測量部分，將它們放到式（15）中算出它們之間的重疊部分：
由式（14）可得卡爾曼增益為： 將式（16）和式（17）的兩邊同時(shí)左乘矩陣的逆（注意里面包含了 ）將其約掉，再將式（16）的第二個(gè)等式兩邊同時(shí)右乘矩陣 的逆得到以下等式： 上式給出了完整的更新步驟方程。就是新的最優(yōu)估計(jì)，我們可以將它和放到下一個(gè)預(yù)測和更新方程中不斷迭代。 以上所有公式中，你只需要用到式（7）、（18）、（19）。（如果忘了的話，你可以根據(jù)式（4）和（15）重新推導(dǎo)一下）
我們可以用這些公式對任何線性系統(tǒng)建立精確的模型，對于非線性系統(tǒng)來說，我們使用擴(kuò)展卡爾曼濾波，區(qū)別在于EKF多了一個(gè)把預(yù)測和測量部分進(jìn)行線性化的過程。ー 9 ー自動駕駛中的傳感器融合算法
追蹤靜止和移動的目標(biāo)是自動駕駛技術(shù)領(lǐng)域最為需要的核心技術(shù)之一。來源于多種傳感器的信號，包括攝像頭，雷達(dá)，以及激光雷達(dá)（基于脈沖激光的測距設(shè)備）等傳感器組合的組合體來估計(jì)位置，速度，軌跡以及目標(biāo)的種類，例如其他車輛和行人。
你可能會問——為什么我們需要這么多的傳感器？
這是因?yàn)槊糠N傳感器提供了追蹤物體所需要的不同精度和類型的信息，尤其是在不同天氣條件下。比如，以激光雷達(dá)為基礎(chǔ)的傳感器能很好地解決位置的問題，但是在糟糕的天氣條件下其精度和性能都會有很大程度的下降。另一方面，雷達(dá)的空間解決方案也相對便宜很多，與此同時(shí)，在惡劣的天氣條件下也能提供更準(zhǔn)確的數(shù)據(jù)。
同樣的，不像激光雷達(dá)傳感器，雷達(dá)可以提供目標(biāo)的速度和方位。雷達(dá)數(shù)據(jù)也是計(jì)算密集型的，因?yàn)橐皇す獍l(fā)射非常多包含每個(gè)獨(dú)立的激光點(diǎn)的范圍的數(shù)據(jù)，它使得你必須理解你的算法。
組合來自不同傳感器信息的技術(shù)稱之為傳感器融合技術(shù)。之所以較早的討論這個(gè)，是因?yàn)閼?yīng)用在傳感器融合之上的算法必須處理短暫的，充滿雜訊的輸入，生成可靠的運(yùn)動狀態(tài)估計(jì)的概率。
下面我們將展示在位置的追蹤和估計(jì)中最通用的算法，卡爾曼濾波器的變種——‘?dāng)U展卡爾曼濾波器’。
1.使用激光雷達(dá)數(shù)據(jù)的基礎(chǔ)卡爾曼濾波器：
卡爾曼濾波器的歷史已經(jīng)超過半個(gè)世紀(jì)，但是對于輸入數(shù)據(jù)的噪聲信息和狀態(tài)估計(jì)的平滑來說仍然是最有效的傳感器融合算法之一。它假定位置參數(shù)是符合高斯分布的，即完全可以被均值和協(xié)方差參數(shù)化:X～N(μ, σ2)
當(dāng)傳感器的信息流開始的時(shí)候,卡爾曼濾波器使用一系列的狀態(tài)信息來預(yù)測和衡量更新步驟去更新被追蹤目標(biāo)的信心值(概率)。預(yù)測和更細(xì)心的步驟如下圖所示：
狀態(tài)預(yù)測：
我們會使用一個(gè)簡化的線性狀態(tài)空間模型(鏈接：去闡述濾波器的工作方式. 一個(gè)系統(tǒng)在t時(shí)刻的線性狀態(tài)可以從t-1時(shí)刻根據(jù)以下等式被估計(jì)：
衡量更新：
卡爾曼濾波器的下一部分則是去使用實(shí)測參數(shù)z去更新預(yù)測狀態(tài)'x',通過縮放因子(通常稱之為卡爾曼增益)成比例的計(jì)算估計(jì)值和測量值之間的誤差。
你可以在以下鏈接中找到衡量更新等式的求導(dǎo)：
舉個(gè)例子
理論部分結(jié)束！讓我們嘗試用一些代碼去表示一些卡爾曼濾波器的基礎(chǔ)過程。
對此，我們模擬一個(gè)目標(biāo)，它的狀態(tài)被四維向量x=[px py vx vy]所描述。
在這個(gè)例子中測量傳感器是返回位置數(shù)據(jù)但是沒有速度信息的激光傳感器。為了觀測到速度信息我們需要使用雷達(dá)傳感器數(shù)據(jù)。在接下來的章節(jié)中，當(dāng)我們討論擴(kuò)展卡爾曼濾波的時(shí)候我們將會涉及到這些。
讓我們以一些假設(shè)開始：
偽代碼：
基礎(chǔ)版本的卡爾曼濾波器代碼步驟列在了下面。你可以找到一個(gè)基礎(chǔ)的例子：
最后的迭代步驟通過測量和應(yīng)用預(yù)測以及濾波器的更新步驟如下所示：
plot_position_variance(x,P,edgecolor='r') #plot initial position and covariance in red
for z in measurements:
x,P =predict(x, P)
x,P = update(x, P,z)
plot_position_variance(x,P,edgecolor='b') #plot updates in blue
print(x)
print(P)
卡爾曼濾波器迭代：濾波器在迭代之后向真實(shí)值收斂
上方的圖闡述了濾波器在每次迭代中狀態(tài)向量的px,py維度和位置的協(xié)方差發(fā)生了哪些變化。紅圈表示初始過程不確定性。隨著預(yù)測和測量更新，我們開始有了較小的誤差（半徑），狀態(tài)估計(jì)開始越來越精確。
如同你看到的，最終估計(jì)的狀態(tài)向量x[11.99,2.05]非常接近最終的觀測值，誤差最小值也縮小到了0.05。
2.擴(kuò)展卡爾曼濾波器——使用雷達(dá)數(shù)據(jù)
雷達(dá)數(shù)據(jù)帶來了一些更困難的挑戰(zhàn)。雷達(dá)返回?cái)?shù)據(jù)值的是基于極坐標(biāo)系，其由三個(gè)部分構(gòu)成：
- ρ /Range(從原點(diǎn)到此的距離)
- ? / bearing ( ρ 和 x的夾角),
- ρ˙：接近率/距離變化率
由于沒有H矩陣將狀態(tài)向量映射到雷達(dá)的測量空間，我們需要一個(gè)函數(shù)h（x）來將狀態(tài)空間銀蛇到測量空間以測量更新步驟。這個(gè)方法是映射極坐標(biāo)到笛卡爾坐標(biāo)的方法的衍生方法，定義如下：
這個(gè)映射闡述了一個(gè)非線性函數(shù)，它將使得卡爾曼濾波器的過程和測量符合高斯分布的假設(shè)無效。擴(kuò)展卡爾曼濾波器使用局部線性模型來逼近非線性模型，然后使用卡爾曼濾波應(yīng)用到逼近值上。局部線性逼近是通過計(jì)算當(dāng)前狀態(tài)估計(jì)的一階泰勒展開得出的。一階的逼近也叫雅克比矩陣。我們不會在這里過多介紹雅克比矩陣的推到過程。相關(guān)內(nèi)容在網(wǎng)絡(luò)中可以找到優(yōu)秀的介紹，但是如果你想要直接使用這些東西，你可以在以下的github引用中找到代碼實(shí)例：Github link
現(xiàn)實(shí)參考
你可以在github存儲庫中找到關(guān)于Kalman過濾器的c++編寫代碼:
代碼示例：
來源：牛喀網(wǎng)
傳感器融合 卡爾曼：NXP傳感器融合筆記08(卡爾曼濾波4)
這一篇文章回歸正題，開始介紹NXP傳感器Sensor Fusion融合方案(以下簡稱NXP方案)中的s算法部分，對應(yīng)最后一個(gè)官方視頻教程和AN5023。如果沒有前期的鋪墊以及卡爾曼基礎(chǔ)理論，看這篇文章肯定會看暈掉。。。做好心理準(zhǔn)了么？！
基本就是翻譯 Optimal State Estimation (2006). 這本書的1.1.3 Matrix calculus 章節(jié)(PS： 這本書是有中文版的，不過翻譯實(shí)在太差了，我還是習(xí)慣看英文版，真的) 另外，這個(gè)筆記需要一些線性代數(shù)的知識。至少是本科學(xué)過線性代數(shù)，而且最近復(fù)習(xí)一遍的水平
連續(xù)時(shí)間線性系統(tǒng)離散化
對于一個(gè)連續(xù)時(shí)間的線性系統(tǒng)：
w為噪聲， G為噪聲驅(qū)動矩陣
其中
是白噪聲的方差強(qiáng)度矩陣 為是狄拉克沖激函數(shù)。
他的離散化形式定義為：
下面直接給出結(jié)論(中間過程可以去看嚴(yán)老師的視頻和書 或者 Simion 的書)
總結(jié)一下：
狀態(tài)轉(zhuǎn)移矩陣 離散化為 噪聲驅(qū)動矩陣離散化為 離散化后噪聲均值為0(如果原來連續(xù)時(shí)間噪聲均值為0的話)， 方差為 舉個(gè)栗子
對于簡單的牛頓力學(xué)方程：
當(dāng)我們學(xué)了微積分之后，可以把速度看成距離的微分，加速度看成速度的微分：
對于這種系統(tǒng)，我們可以將設(shè)狀態(tài)向量為
這是一個(gè)一階微分方程，令 ，則：
最終得到：
常見的簡單隨機(jī)過程離散化
其中：
為反相關(guān)時(shí)間常數(shù) 為激勵(lì)高斯白噪聲實(shí)際物理信號的噪聲單位
這一節(jié)解釋一下 啥叫隨機(jī)游走，噪聲密度，以及他們的單位到底是怎么推導(dǎo)出來的：
U表示任意物理量
在物理上，要考慮單位, 連續(xù)時(shí)間噪聲的單位 應(yīng)該是 U/s(U代表任意物理量，比如距離單位m ，速度單位m/s 等等) 那么噪聲的方差就應(yīng)該是 (U/s)^(2)就是 。 第二點(diǎn)，要知道沖擊響應(yīng)函數(shù) 單位是 , 所以功率譜密度的單位就是 。物理意義就是每Hz上分配的功率大小，就是功率譜的概念。
把 開根號就得到 激勵(lì)噪聲密度：
激勵(lì)噪聲密度 和 隨機(jī)游走系數(shù) 是等價(jià)的單位相同，只是習(xí)慣于描述不同的物理量，激勵(lì)噪聲密度描述w噪聲本身，而隨機(jī)游走系數(shù)是想描述x(物理量本身)
慣導(dǎo)中常用誤差：
卡爾曼濾波復(fù)習(xí)
簡單重溫下標(biāo)準(zhǔn)的卡爾曼濾波：NXP使用的符號可能和前面不一樣，比如量測矩陣H，在NXP這里叫做C，其他大部分都一樣：
標(biāo)準(zhǔn)線性卡爾曼復(fù)習(xí)，A為過程轉(zhuǎn)移矩陣，C為量測矩陣（其他文章一般叫做H）
幾點(diǎn)需要注意：
減號代表 priori，中文叫做先驗(yàn)(感覺中文防翻譯的并不好)。 其實(shí)就是卡爾曼公式中在新的量測數(shù)據(jù)到達(dá)之前的各個(gè)狀態(tài)量加號代表posteriori, 中文叫做后驗(yàn)。其實(shí)就是卡爾曼公式中在新的量測數(shù)據(jù)到達(dá)之后的，進(jìn)行更新之后的各個(gè)狀態(tài)量
對于NXP的sensor fusion 方案，有如下幾個(gè)特點(diǎn)
總結(jié)以下：
NXP的方案是間接(誤差)卡爾曼濾波NXP的方案過程轉(zhuǎn)移矩陣A為0！這會導(dǎo)致卡爾曼的5個(gè)公式大變!,真正的開源代碼和AN5023中有一點(diǎn)點(diǎn)不一樣。符號表
最好把這個(gè)符號表打出來，供后面分析使用.
1. 加速度計(jì)和地磁的噪聲方差加速度噪聲方差矩陣
具體推導(dǎo)過程可以參考AN筆記，這里直接寫下結(jié)論，就是加速度計(jì)和地磁的噪聲方差認(rèn)為等于三倍的與正常值的模(當(dāng)?shù)貥?biāo)準(zhǔn)重力大小，當(dāng)?shù)貥?biāo)準(zhǔn)地磁場大小)的差。
加速度所有噪聲方差
其中:
:加速度計(jì)噪聲的方差: :線性加速度干擾說造成的方差: 地磁噪聲偏差矩陣地磁噪聲方差
至于這個(gè)3是怎么來的，AN5023里有寫，我就不推導(dǎo)了
陀螺和加速度計(jì)誤差向量估計(jì)
在NXP sensor fusion 中 旋轉(zhuǎn)狀態(tài)指的是 Goblal->Local既：
對于重力有：
對于地磁有：
兩個(gè)向量r,s ,計(jì)算從u轉(zhuǎn)到v的四元數(shù)的公式為：
計(jì)算從向量r轉(zhuǎn)到向量s的四元數(shù),其中r和s必須是單位化的
這個(gè)q的矢量部分可以表示兩個(gè)向量之間差的夾角。在后面NXP的方案中會頻繁用到。
重力及地磁誤差四元數(shù)
加速度計(jì)四元數(shù)誤差，其實(shí)就是38式子，取其中的矢量部分：
加速度誤差四元數(shù)
取其中矢量部分：
加速度誤差四元數(shù)的矢量部分
同理可得地磁場的誤差四元數(shù)：
地磁場誤差四元數(shù)陀螺儀傳感器模型
記陀螺儀讀值為 ，則認(rèn)為陀螺儀測量模型為：
陀螺儀測量模型
其中：
是真實(shí)的角速度 陀螺零偏 是陀螺噪聲(認(rèn)為是高斯噪聲)
陀螺噪聲表達(dá)為：
陀螺噪聲方差
陀螺零偏 又可以表示為上一次的零偏加一個(gè)高斯噪聲(隨機(jī)游走)：
陀螺零偏模型(是一個(gè)隨機(jī)游走過程)
其中 也是高斯噪聲，三個(gè)軸之前無獨(dú)立，表示為：
陀螺隨機(jī)游走噪聲表示角速度誤差的模型
注意這里是角速度誤差的模型，不是角速度的模型！
其中
是上一次的角速度誤差估計(jì)值 加速度隨機(jī)游走 角速度噪聲
編譯時(shí)的宏常量
FQVY_6DOF_GBY_KALMAN: 陀螺噪聲，這個(gè)值越大，陀螺置信度越低，系統(tǒng)更依賴于觀測值(加速度，地磁)。所有會導(dǎo)致收斂更快，但更容易被加速度，地磁噪聲影響。 QWB_6DOF_GY_KALMAN： 陀螺零偏噪聲： 這個(gè)值越大，零偏跟蹤性能越好。但是更受噪聲影響。直接卡爾曼過程模型：(以下以6軸進(jìn)行講解，先不考慮地磁)
狀態(tài)量：
直接卡爾曼狀態(tài)量
狀態(tài)轉(zhuǎn)移就是四元數(shù)積分：
其中： (推導(dǎo)見前面四元數(shù)筆記)
間接(誤差)卡爾曼過程模型
NXP開源代碼實(shí)用的是誤差卡爾曼，而非直接卡爾曼！
狀態(tài)量：
間接卡爾曼狀態(tài)量，注意，最后的b是陀螺零偏的誤差, 不是陀螺零偏本身！
兩個(gè)結(jié)論：
長期于高速運(yùn)動時(shí)候(設(shè)地磁干擾很小)，只有地磁作為量測，加速度計(jì)因?yàn)橛芯薮蟮木€性加速度干擾導(dǎo)致噪聲會非常大，長期處于這種運(yùn)動中，會造成pitch/ roll飄移
長期處于地磁干擾情況下(低速運(yùn)動或者靜止)，只有加速度計(jì)參與量測更新，Pitch和Roll可以得到量測更新但是Yaw得客觀性為0，所以Yaw會緩慢飄逸。
卡爾曼方程：
過程轉(zhuǎn)移方程： ，因?yàn)槊恳淮胃潞笳J(rèn)為誤差為0，所以有：
每次更新后認(rèn)為誤差為0，所以過程轉(zhuǎn)移矩陣直接沒了。。。
卡爾曼狀態(tài)量更新方程直接變成：
這個(gè)感覺實(shí)在是差強(qiáng)人意啊，上一步的狀態(tài)量直接就沒了。。這還能時(shí)卡爾曼？。。。
再來看系統(tǒng)方差矩陣，卡爾曼原來的先驗(yàn)系統(tǒng)方差為： ，因?yàn)楝F(xiàn)在A為0，所以。。：
系統(tǒng)協(xié)方差矩陣直接等于過程方差
卡爾曼增益K的標(biāo)準(zhǔn)公式為： (C就是H，量測矩陣)，因?yàn)楝F(xiàn)在 ，所以卡爾曼增益公式變成：
卡爾曼增益公式
由此可見，NXP這套方案主要取決于兩個(gè)方差矩陣：
過程方差，陀螺儀的噪聲。 量測方差，加速度和地磁的噪聲。
當(dāng) 很大時(shí)候(高動態(tài)，高地磁干擾)，卡爾曼系數(shù)趨近于： ，所以更多相信陀螺儀： 。反之，當(dāng) 過大的時(shí)候，系統(tǒng)更相信測量值，卡爾曼系數(shù)接近于1：狀態(tài)量更基本取決于量測：
也就是說，只要確定了 和 和 ,整個(gè)系統(tǒng)的每個(gè)方程就確定下來了: 下面就來搞定 和 和 ！！！
量測矩陣 量測方差 和過程方差 的確定
的確定
6軸融合中， 量測輸入為 ，這是重力誤差四元數(shù)的矢量部分(陀螺儀算出來的先驗(yàn)重力向量和加速度測出的重力向量的誤差四元數(shù)矢量部分)
卡爾曼量測更新，注意因?yàn)檫^程矩陣A已經(jīng)沒了，所以量測更新變成了現(xiàn)在這個(gè)模樣。。
重力量測方程：
記 則有：
6軸誤差卡爾曼量測方程H(在NXP官方筆記里叫做C)9軸誤差卡爾曼量測方程H(在NXP官方筆記里叫做C)
的確定：
對于9軸的量測方差 ：
9軸量測方差矩陣的確定， Qvk是一個(gè)6x6矩陣
因?yàn)榱繙y輸入 既和陀螺的噪聲，陀螺的零偏有關(guān)系，也和加速度的噪聲，加速度的零偏有關(guān)系，所以有：
量測噪聲Qv
對于6軸 注意 是一個(gè)3x3矩陣，并且對角線元素都一樣：
6軸量測方差矩陣
過程方差 的確定
9軸過程噪聲方差的定義
6軸過程噪聲方差的定義
下面來一個(gè)一個(gè)看里面的內(nèi)容：
先研究 ：它是陀螺零篇的估計(jì)誤差，可以表示為：
其中 表示為：
再研究 ：它是地磁誤差四元數(shù)和自己的協(xié)方差:
其中 為：
再研究 ：它是重力矯正四元數(shù)矢量部分的誤差，可以表示為：
其中 表示為：
再研究 ：它是重力矯正四元數(shù)和陀螺零篇誤差之間的協(xié)方差：
其中 表示為：
再研究 ：它是地磁矯正四元數(shù)和陀螺零偏誤差之間的協(xié)方差：
其中 為
至此，過程方差全部確定。
一些總結(jié)：
總框圖一些重點(diǎn)總結(jié)一些重點(diǎn)總結(jié)NXP方案的量測輸入時(shí)先驗(yàn)重力誤差四元數(shù)(上一次姿態(tài)算出來的重力估計(jì)值和本次加速度計(jì)讀回來的重力值得誤差四元數(shù))和 先驗(yàn)地磁場誤差四元數(shù)。 量測噪聲本身和重力和地磁場的模值有關(guān)系。如果和當(dāng)?shù)刂亓?或者 地磁場大小差的太大，表明目前干擾很大計(jì)算過程方差 非常復(fù)雜，詳見AN5023
傳感器融合 卡爾曼：卡爾曼濾波器是如何運(yùn)用于多傳感器融合的？
我目前認(rèn)為的卡爾曼濾波器是這樣的：
1.設(shè)置狀態(tài)轉(zhuǎn)移矩陣
2.[預(yù)測]通過 t-1 時(shí)的狀態(tài)預(yù)測 t 時(shí)的狀態(tài)
3.[更新]用傳感器的測量值與 2 中的預(yù)測值估計(jì)出最優(yōu)值，并更新卡爾曼增益等常數(shù)
4.重復(fù) 2～3 步驟
（如上正是opencv庫中自帶的卡爾曼濾波器例子的步驟，也是初學(xué)者能最直觀理解到的，它只涉及到一個(gè)測量值）
那么請問，在某些涉及多傳感器融合的問題中，例如 陀螺儀（累積誤差）和電子羅盤（干擾）的數(shù)據(jù)融合，此時(shí)有多個(gè)測量值存在，卡爾曼濾波器又是怎樣工作的呢？
貼一個(gè)數(shù)據(jù)融合的結(jié)果，用陀螺儀和GPS數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，濾波估計(jì)車輛前輪轉(zhuǎn)角即行業(yè)內(nèi)所說的慣導(dǎo)角位移。LZ精準(zhǔn)農(nóng)業(yè)領(lǐng)域，器件型號不表，用霍爾傳感器(精度0.1度)標(biāo)定，精度在0.17度，用于農(nóng)機(jī)自動駕駛，真他娘的好用。。。。。。
補(bǔ)一個(gè)IMU姿態(tài)測量系統(tǒng)數(shù)據(jù)融合的結(jié)果。陀螺和加表數(shù)據(jù)融合，濾波估計(jì)系統(tǒng)俯仰角和橫滾角。器件型號不表。
這個(gè)是實(shí)驗(yàn)室桌面實(shí)驗(yàn)，手動翻轉(zhuǎn)IMU
下面是室外實(shí)驗(yàn)，用GPS雙天線的俯仰角進(jìn)行精度標(biāo)定，GPS主副天線基線2米，俯仰角精度0.1度。IMU剛性鏈接在GPS工裝上，兩手托舉行走。
更一個(gè)濾波融合的例子。
霍爾傳感器安裝在車輛輪子處測量車輛前輪的角度，但是霍爾傳感器測量的是0到360度的絕對角度，安裝在車輪轉(zhuǎn)向節(jié)處時(shí)，在車輛前輪打正的情況下角度不是0而是一個(gè)隨機(jī)的固定的偏置，采用濾波估計(jì)的方法估計(jì)其安裝偏置，精度可以達(dá)到0.1度：
再更一個(gè)IMU或者GPS在載體上安裝角估計(jì)的應(yīng)用。IMU或者雙天線GPS安裝在載體上，由于安裝誤差，IMU的前向或GPS主副天線的航向與載體的前向有一個(gè)固定的安裝誤差角，采用濾波估計(jì)的方法進(jìn)行估計(jì)，精度可以達(dá)到0.2度：
在更一個(gè)組合導(dǎo)航的例子。
單天線GNSS+IMU做組合導(dǎo)航，提供位置速度姿態(tài)，IMU型號是ADI某較低成本器件，位置速度精度和RTK狀態(tài)下GNSS結(jié)果保持一致，航向精度0.12度(RMS)。
產(chǎn)品是農(nóng)機(jī)自動駕駛儀，用來輔助農(nóng)機(jī)的自動駕駛進(jìn)行作業(yè)，目前主要應(yīng)用在插秧機(jī)、拖拉機(jī)、割草機(jī)等四輪小車。
組合航向精度和雙天線GNSS航向精度基本一致，且靜止情況下航向不漂。
農(nóng)機(jī)橫向控制精度<2.5cm RMS。
傳感器融合 卡爾曼：擴(kuò)展卡爾曼濾波EKF與多傳感器融合
參考：
Extended Kalman Filter（擴(kuò)展卡爾曼濾波）是卡爾曼濾波的非線性版本。在狀態(tài)轉(zhuǎn)移方程確定的情況下，EKF已經(jīng)成為了非線性系統(tǒng)狀態(tài)估計(jì)的事實(shí)標(biāo)準(zhǔn)。本文將簡要介紹EKF，并介紹其在無人駕駛多傳感器融合上的應(yīng)用。
KF與EKF
本文假定讀者已熟悉KF，若不熟悉請參考卡爾曼濾波簡介。
KF與EKF的區(qū)別如下：
預(yù)測未來：x′=Fx+u用x′=f(x,u)代替；其余F用Fj代替。
修正當(dāng)下：將狀態(tài)映射到測量的Hx′用h(x′)代替；其余H用Hj代替。
其中，非線性函數(shù)f(x,u)，h(x′)用非線性得到了更精準(zhǔn)的狀態(tài)預(yù)測值、映射后的測量值；線性變換Fj，，Hj通過線性變換使得變換后的x，z仍滿足高斯分布的假設(shè)。
Fj，Hj計(jì)算方式如下：
為什么要用EKF
KF的假設(shè)之一就是高斯分布的x預(yù)測后仍服從高斯分布，高斯分布的x變換到測量空間后仍服從高斯分布。可是，假如F、H是非線性變換，那么上述條件則不成立。
將非線性系統(tǒng)線性化
既然非線性系統(tǒng)不行，那么很自然的解決思路就是將非線性系統(tǒng)線性化。
對于一維系統(tǒng)，采用泰勒一階展開即可得到：
對于多維系統(tǒng)，仍舊采用泰勒一階展開即可得到：
其中，Df(a)是Jacobian矩陣。
多傳感器融合
lidar與radar
本文將以汽車跟蹤為例，目標(biāo)是知道汽車時(shí)刻的狀態(tài)
。已知的傳感器有l(wèi)idar、radar。
lidar：笛卡爾坐標(biāo)系。可檢測到位置，沒有速度信息。其測量值
。
radar：極坐標(biāo)系。可檢測到距離，角度，速度信息，但是精度較低。其測量值
，圖示如下。
傳感器融合步驟
步驟圖如上所示，包括：
收到第一個(gè)測量值，對狀態(tài)xx進(jìn)行初始化。
預(yù)測未來
修正當(dāng)下
初始化
初始化，指在收到第一個(gè)測量值后，對狀態(tài)x進(jìn)行初始化。初始化如下，同時(shí)加上對時(shí)間的更新。
對于radar來說，
對于radar來說，
預(yù)測未來
預(yù)測主要涉及的公式是：
需要求解的有三個(gè)變量：F、P、Q。
F表明了系統(tǒng)的狀態(tài)如何改變，這里僅考慮線性系統(tǒng)，F(xiàn)易得：
P表明了系統(tǒng)狀態(tài)的不確定性程度，用x的協(xié)方差表示，這里自己指定為：
Q表明了x′=Fx未能刻畫的其他外界干擾。本例子使用線性模型，因此加速度變成了干擾項(xiàng)。x′=Fx中未衡量的額外項(xiàng)目v為：
v服從高斯分布N(0,Q)。
修正當(dāng)下
lidar
lidar使用了KF。修正當(dāng)下這里牽涉到的公式主要是：
需要求解的有兩個(gè)變量：H、R。
H表示了狀態(tài)空間到測量空間的映射。
R表示了測量值的不確定度，一般由傳感器的廠家提供，這里lidar參考如下：
radar
radar使用了EKF。修正當(dāng)下這里牽涉到的公式主要是：
區(qū)別與上面lidar的主要有：
狀態(tài)空間到測量空間的非線性映射f(x)
非線性映射線性化后的Jacob矩陣
radar的
狀態(tài)空間到測量空間的非線性映射f(x)如下
非線性映射線性化后的Jacob矩陣Hj
R表示了測量值的不確定度，一般由傳感器的廠家提供，這里radar參考如下：
傳感器融合實(shí)例
多傳感器融合的示例如下，需要注意的有：
lidar和radar的預(yù)測部分是完全相同的
lidar和radar的參數(shù)更新部分是不同的，不同的原因是不同傳感器收到的測量值是不同的
當(dāng)收到lidar或radar的測量值，依次執(zhí)行預(yù)測、更新步驟
當(dāng)同時(shí)收到lidar和radar的測量值，依次執(zhí)行預(yù)測、更新1、更新2步驟
多傳感器融合的效果如下圖所示，紅點(diǎn)和藍(lán)點(diǎn)分別表示radar和lidar的測量位置，綠點(diǎn)代表了EKF經(jīng)過多傳感器融合后獲取到的測量位置，取得了較低的RMSE。
