卡爾曼濾波傳感器融合：1.使用激光雷達(dá)數(shù)據(jù)的基礎(chǔ)卡爾曼濾波器：
展開(kāi)全文
卡爾曼濾波器是傳感器融合工程師用于自動(dòng)駕駛汽車的工具。想象一下，你有一個(gè)雷達(dá)傳感器，告訴你另一輛車距離15米，一個(gè)激光傳感器說(shuō)車輛距離20米。你如何協(xié)調(diào)這些傳感器測(cè)量？這就是卡爾曼濾波器的功能。卡爾曼濾波在自動(dòng)駕駛汽車上的應(yīng)用十分廣泛，本文講述卡爾曼濾波算法，希望對(duì)你有所幫助。
卡爾曼濾波算法在控制領(lǐng)域有極廣泛的應(yīng)用，在發(fā)動(dòng)機(jī)燃油噴射控制中，可以應(yīng)用擴(kuò)展的卡爾曼濾波理論研究瞬態(tài)工況下發(fā)動(dòng)機(jī)循環(huán)進(jìn)氣量的最優(yōu)估計(jì)算法，在雷達(dá)中，人們感興趣的是跟蹤目標(biāo)，但目標(biāo)的位置、速度、加速度的測(cè)量值往往在任何時(shí)候都有噪聲。卡爾曼濾波利用目標(biāo)的動(dòng)態(tài)信息,設(shè)法去掉噪聲的影響，得到一個(gè)關(guān)于目標(biāo)位置的好的估計(jì)。
為了以后更好的工程實(shí)踐應(yīng)用卡爾曼濾波算法，今天小編帶領(lǐng)著大家了解卡爾曼濾波算法的理論，及其在自動(dòng)駕駛多傳感器融合算法中的應(yīng)用。ー 1 ー什么是卡爾曼濾波？
你可以在任何含有不確定信息的動(dòng)態(tài)系統(tǒng)中使用卡爾曼濾波，對(duì)系統(tǒng)下一步的走向做出有根據(jù)的預(yù)測(cè)，即使伴隨著各種干擾，卡爾曼濾波總是能指出真實(shí)發(fā)生的情況。
在連續(xù)變化的系統(tǒng)中使用卡爾曼濾波是非常理想的，它具有占用內(nèi)存小的優(yōu)點(diǎn)（除了前一個(gè)狀態(tài)量外，不需要保留其它歷史數(shù)據(jù)），并且速度很快，很適合應(yīng)用于實(shí)時(shí)問(wèn)題和嵌入式系統(tǒng)。
在Google上找到的大多數(shù)關(guān)于實(shí)現(xiàn)卡爾曼濾波的數(shù)學(xué)公式看起來(lái)有點(diǎn)晦澀難懂，這個(gè)狀況有點(diǎn)糟糕。實(shí)際上，如果以正確的方式看待它，卡爾曼濾波是非常簡(jiǎn)單和容易理解的，下面我將用漂亮的圖片和色彩清晰的闡述它，你只需要懂一些基本的概率和矩陣的知識(shí)就可以了。
ー 2 ー我們能用卡爾曼濾波做什么？
用玩具舉例：你開(kāi)發(fā)了一個(gè)可以在樹(shù)林里到處跑的小機(jī)器人，這個(gè)機(jī)器人需要知道它所在的確切位置才能導(dǎo)航。我們可以說(shuō)機(jī)器人有一個(gè)狀態(tài)： ，表示位置和速度：
注意這個(gè)狀態(tài)只是關(guān)于這個(gè)系統(tǒng)基本屬性的一堆數(shù)字，它可以是任何其它的東西。在這個(gè)例子中是位置和速度，它也可以是一個(gè)容器中液體的總量，汽車發(fā)動(dòng)機(jī)的溫度，用戶手指在觸摸板上的位置坐標(biāo)，或者任何你需要跟蹤的信號(hào)。這個(gè)機(jī)器人帶有GPS，精度大約為10米，還算不錯(cuò)，但是，它需要將自己的位置精確到10米以內(nèi)。樹(shù)林里有很多溝壑和懸崖，如果機(jī)器人走錯(cuò)了一步，就有可能掉下懸崖，所以只有GPS是不夠的。或許我們知道一些機(jī)器人如何運(yùn)動(dòng)的信息：例如，機(jī)器人知道發(fā)送給電機(jī)的指令，知道自己是否在朝一個(gè)方向移動(dòng)并且沒(méi)有人干預(yù)，在下一個(gè)狀態(tài)，機(jī)器人很可能朝著相同的方向移動(dòng)。當(dāng)然，機(jī)器人對(duì)自己的運(yùn)動(dòng)是一無(wú)所知的：它可能受到風(fēng)吹的影響，輪子方向偏了一點(diǎn)，或者遇到不平的地面而翻倒。所以，輪子轉(zhuǎn)過(guò)的長(zhǎng)度并不能精確表示機(jī)器人實(shí)際行走的距離，預(yù)測(cè)也不是很完美。GPS 傳感器告訴了我們一些狀態(tài)信息，我們的預(yù)測(cè)告訴了我們機(jī)器人會(huì)怎樣運(yùn)動(dòng)，但都只是間接的，并且伴隨著一些不確定和不準(zhǔn)確性。但是，如果使用所有對(duì)我們可用的信息，我們能得到一個(gè)比任何依據(jù)自身估計(jì)更好的結(jié)果嗎？回答當(dāng)然是YES，這就是卡爾曼濾波的用處。ー 3 ー卡爾曼濾波是如何看到你的問(wèn)題的？
下面我們繼續(xù)以只有位置和速度這兩個(gè)狀態(tài)的簡(jiǎn)單例子做解釋。我們并不知道實(shí)際的位置和速度，它們之間有很多種可能正確的組合，但其中一些的可能性要大于其它部分： 卡爾曼濾波假設(shè)兩個(gè)變量（位置和速度，在這個(gè)例子中）都是隨機(jī)的，并且服從高斯分布。每個(gè)變量都有一個(gè)均值μ，表示隨機(jī)分布的中心（最可能的狀態(tài)），以及方差，表示不確定性。在上圖中，位置和速度是不相關(guān)的，這意味著由其中一個(gè)變量的狀態(tài)無(wú)法推測(cè)出另一個(gè)變量可能的值。下面的例子更有趣：位置和速度是相關(guān)的，觀測(cè)特定位置的可能性取決于當(dāng)前的速度： 這種情況是有可能發(fā)生的，例如，我們基于舊的位置來(lái)估計(jì)新位置。如果速度過(guò)高，我們可能已經(jīng)移動(dòng)很遠(yuǎn)了。如果緩慢移動(dòng)，則距離不會(huì)很遠(yuǎn)。跟蹤這種關(guān)系是非常重要的，因?yàn)樗鼛Ыo我們更多的信息：其中一個(gè)測(cè)量值告訴了我們其它變量可能的值，這就是卡爾曼濾波的目的，盡可能地在包含不確定性的測(cè)量數(shù)據(jù)中提取更多信息！ ：這種相關(guān)性用協(xié)方差矩陣來(lái)表示，簡(jiǎn)而言之，矩陣中的每個(gè)元素， 表示第 i 個(gè)和第 j 個(gè)狀態(tài)變量之間的相關(guān)度。（你可能已經(jīng)猜到協(xié)方差矩陣是一個(gè)對(duì)稱矩陣，這意味著可以任意交換 i 和 j）。協(xié)方差矩陣通常用來(lái)表示，其中的元素則表示為ー 4 ー使用矩陣來(lái)描述問(wèn)題
我們基于高斯分布來(lái)建立狀態(tài)變量，所以在時(shí)刻 k 需要兩個(gè)信息：最佳估計(jì)
（即均值，其它地方常用 μ 表示），以及協(xié)方差矩陣。
（當(dāng)然，在這里我們只用到了位置和速度，實(shí)際上這個(gè)狀態(tài)可以包含多個(gè)變量，代表任何你想表示的信息）。接下來(lái)，我們需要根據(jù)當(dāng)前狀態(tài)（k-1 時(shí)刻）來(lái)預(yù)測(cè)下一狀態(tài)（k 時(shí)刻）。記住，我們并不知道對(duì)下一狀態(tài)的所有預(yù)測(cè)中哪個(gè)是“真實(shí)”的，但我們的預(yù)測(cè)函數(shù)并不在乎。它對(duì)所有的可能性進(jìn)行預(yù)測(cè)，并給出新的高斯分布。我們可以用矩陣來(lái)表示這個(gè)預(yù)測(cè)過(guò)程：
它將我們?cè)脊烙?jì)中的每個(gè)點(diǎn)都移動(dòng)到了一個(gè)新的預(yù)測(cè)位置，如果原始估計(jì)是正確的話，這個(gè)新的預(yù)測(cè)位置就是系統(tǒng)下一步會(huì)移動(dòng)到的位置。那我們又如何用矩陣來(lái)預(yù)測(cè)下一個(gè)時(shí)刻的位置和速度呢？下面用一個(gè)基本的運(yùn)動(dòng)學(xué)公式來(lái)表示：現(xiàn)在，我們有了一個(gè)預(yù)測(cè)矩陣來(lái)表示下一時(shí)刻的狀態(tài)，但是，我們?nèi)匀徊恢涝趺锤聟f(xié)方差矩陣。此時(shí)，我們需要引入另一個(gè)公式，如果我們將分布中的每個(gè)點(diǎn)都乘以矩陣 A，它的協(xié)方差矩陣系會(huì)怎樣變化呢？很簡(jiǎn)單，下面給出公式：結(jié)合方程（4）和（3）得到： ー 5 ー外部控制量我們并沒(méi)有捕捉到一切信息，可能存在外部因素會(huì)對(duì)系統(tǒng)進(jìn)行控制，帶來(lái)一些與系統(tǒng)自身狀態(tài)沒(méi)有相關(guān)性的改變。 以火車的運(yùn)動(dòng)狀態(tài)模型為例，火車司機(jī)可能會(huì)操縱油門，讓火車加速。相同地，在我們機(jī)器人這個(gè)例子中，導(dǎo)航軟件可能會(huì)發(fā)出一個(gè)指令讓輪子轉(zhuǎn)向或者停止。如果知這些額外的信息，我們可以用一個(gè)向量來(lái)表示，將它加到我們的預(yù)測(cè)方程中做修正。假設(shè)由于油門的設(shè)置或控制命令，我們知道了期望的加速度，根據(jù)基本的運(yùn)動(dòng)學(xué)方程可以得到：
以矩陣的形式表示就是： 稱為控制矩陣，稱為控制向量（對(duì)于沒(méi)有外部控制的簡(jiǎn)單系統(tǒng)來(lái)說(shuō)，這部分可以忽略）。讓我們?cè)偎伎家幌拢绻覀兊念A(yù)測(cè)并不是100%準(zhǔn)確的，該怎么辦呢？ー 6 ー外部干擾如果這些狀態(tài)量是基于系統(tǒng)自身的屬性或者已知的外部控制作用來(lái)變化的，則不會(huì)出現(xiàn)什么問(wèn)題。但是，如果存在未知的干擾呢？例如，假設(shè)我們跟蹤一個(gè)四旋翼飛行器，它可能會(huì)受到風(fēng)的干擾，如果我們跟蹤一個(gè)輪式機(jī)器人，輪子可能會(huì)打滑，或者路面上的小坡會(huì)讓它減速。這樣的話我們就不能繼續(xù)對(duì)這些狀態(tài)進(jìn)行跟蹤，如果沒(méi)有把這些外部干擾考慮在內(nèi)，我們的預(yù)測(cè)就會(huì)出現(xiàn)偏差。 在每次預(yù)測(cè)之后，我們可以添加一些新的不確定性來(lái)建立這種與“外界”（即我們沒(méi)有跟蹤的干擾）之間的不確定性模型： 原始估計(jì)中的每個(gè)狀態(tài)變量更新到新的狀態(tài)后，仍然服從高斯分布。我們可以說(shuō)的每個(gè)狀態(tài)變量移動(dòng)到了一個(gè)新的服從高斯分布的區(qū)域，協(xié)方差為
。換句話說(shuō)就是，我們將這些沒(méi)有被跟蹤的干擾當(dāng)作是協(xié)方差的的噪聲來(lái)處理。這產(chǎn)生了具有不同協(xié)方差（但是具有相同的均值）的新的高斯分布。我們通過(guò)簡(jiǎn)單地添加得到的擴(kuò)展的協(xié)方差，下面給出預(yù)測(cè)步驟的完整表達(dá)式：由上式可知，新的最優(yōu)估計(jì)是根據(jù)上一個(gè)最優(yōu)估計(jì)預(yù)測(cè)的道德，并加上已知外部控制量的修正。而新的不確定性由上一不確定性預(yù)測(cè)得到，并加上外部環(huán)境的干擾。好了，我們對(duì)系統(tǒng)可能的動(dòng)向有了一個(gè)模糊的估計(jì)，用和來(lái)表示。如果結(jié)合傳感器的數(shù)據(jù)會(huì)怎樣呢？
ー 7 ー用測(cè)量值來(lái)修正估計(jì)值我們可能會(huì)有多個(gè)傳感器來(lái)測(cè)量系統(tǒng)當(dāng)前的狀態(tài)，哪個(gè)傳感器具體測(cè)量的是哪個(gè)狀態(tài)變量并不重要，也許一個(gè)是測(cè)量位置，一個(gè)是測(cè)量速度，每個(gè)傳感器間接地告訴了我們一些狀態(tài)信息。注意，傳感器讀取的數(shù)據(jù)的單位和尺度有可能與我們要跟蹤的狀態(tài)額單位和尺度不一樣，我們用矩陣來(lái)表示傳感器的數(shù)據(jù)。我們可以計(jì)算出傳感器讀數(shù)的分布，用之前的表示方法如下式所示： 卡爾曼濾波的一大優(yōu)點(diǎn)就是能處理傳感器噪聲，換句話說(shuō)，我們的傳感器或多或少都有點(diǎn)不可靠，并且原始估計(jì)中的每個(gè)狀態(tài)可以和一定范圍內(nèi)的傳感器讀數(shù)對(duì)應(yīng)起來(lái)。
從測(cè)量到的傳感器數(shù)據(jù)中，我們大致能猜到系統(tǒng)當(dāng)前處于什么狀態(tài)。但是由于存在不確定性，某些狀態(tài)可能比我們得到的讀數(shù)更接近真實(shí)狀態(tài)。
我們將這種不確定性（例如：傳感器噪聲）用協(xié)方差表示，該分布的均值就是我們讀取到的傳感器數(shù)據(jù)，稱之為：傳感器噪 現(xiàn)在我們有了兩個(gè)高斯分布，一個(gè)是在預(yù)測(cè)值附近，一個(gè)是在傳感器讀數(shù)附近。我們必須在預(yù)測(cè)值（粉紅色）和傳感器測(cè)量值（綠色）之間找到最優(yōu)解。 那么，我們最有可能的狀態(tài)是什么呢？對(duì)于任何可能的度數(shù)有兩種情況：（1）傳感器的測(cè)量值；（2）由前一狀態(tài)得到的預(yù)測(cè)值。如果我們想知道這兩種情況都可能發(fā)生的概率，將這兩個(gè)高斯分布相乘就可以了。剩下的就是重疊部分了，這個(gè)重疊部分的均值就是兩個(gè)估計(jì)最可能的值，也就是給定的所有信息中的最優(yōu)估計(jì)。瞧！這個(gè)重疊的區(qū)域看起來(lái)像另一個(gè)高斯分布。如你所見(jiàn)，把兩個(gè)具有不同均值和方差的高斯分布相乘，你會(huì)得到一個(gè)新的具有獨(dú)立均值和方差的高斯分布！下面用公式講解。ー 8 ー融合高斯分布先以一維高斯分布來(lái)分析比較簡(jiǎn)單點(diǎn)，具有方差 和 μ 的高斯曲線可以用下式表示：
如果把兩個(gè)服從高斯分布的函數(shù)相乘會(huì)得到什么呢？
將式（9）代入到式（10）中（注意重新歸一化，使總概率為1）可以得到： 將式（11）中的兩個(gè)式子相同的部分用 k 表示：
下面進(jìn)一步將式（12）和（13）寫成矩陣的形式，如果 Σ 表示高斯分布的協(xié)方差， 表示每個(gè)維度的均值，則：
矩陣稱為卡爾曼增益，下面將會(huì)用到。放松！我們快要完成了！
將所有公式結(jié)合起來(lái)我們有兩個(gè)高斯分布，預(yù)測(cè)部分，和測(cè)量部分，將它們放到式（15）中算出它們之間的重疊部分：
由式（14）可得卡爾曼增益為： 將式（16）和式（17）的兩邊同時(shí)左乘矩陣的逆（注意里面包含了 ）將其約掉，再將式（16）的第二個(gè)等式兩邊同時(shí)右乘矩陣 的逆得到以下等式： 上式給出了完整的更新步驟方程。就是新的最優(yōu)估計(jì)，我們可以將它和放到下一個(gè)預(yù)測(cè)和更新方程中不斷迭代。 以上所有公式中，你只需要用到式（7）、（18）、（19）。（如果忘了的話，你可以根據(jù)式（4）和（15）重新推導(dǎo)一下）
我們可以用這些公式對(duì)任何線性系統(tǒng)建立精確的模型，對(duì)于非線性系統(tǒng)來(lái)說(shuō)，我們使用擴(kuò)展卡爾曼濾波，區(qū)別在于EKF多了一個(gè)把預(yù)測(cè)和測(cè)量部分進(jìn)行線性化的過(guò)程。ー 9 ー自動(dòng)駕駛中的傳感器融合算法
追蹤靜止和移動(dòng)的目標(biāo)是自動(dòng)駕駛技術(shù)領(lǐng)域最為需要的核心技術(shù)之一。來(lái)源于多種傳感器的信號(hào)，包括攝像頭，雷達(dá)，以及激光雷達(dá)（基于脈沖激光的測(cè)距設(shè)備）等傳感器組合的組合體來(lái)估計(jì)位置，速度，軌跡以及目標(biāo)的種類，例如其他車輛和行人。
你可能會(huì)問(wèn)——為什么我們需要這么多的傳感器？
這是因?yàn)槊糠N傳感器提供了追蹤物體所需要的不同精度和類型的信息，尤其是在不同天氣條件下。比如，以激光雷達(dá)為基礎(chǔ)的傳感器能很好地解決位置的問(wèn)題，但是在糟糕的天氣條件下其精度和性能都會(huì)有很大程度的下降。另一方面，雷達(dá)的空間解決方案也相對(duì)便宜很多，與此同時(shí)，在惡劣的天氣條件下也能提供更準(zhǔn)確的數(shù)據(jù)。
同樣的，不像激光雷達(dá)傳感器，雷達(dá)可以提供目標(biāo)的速度和方位。雷達(dá)數(shù)據(jù)也是計(jì)算密集型的，因?yàn)橐皇す獍l(fā)射非常多包含每個(gè)獨(dú)立的激光點(diǎn)的范圍的數(shù)據(jù)，它使得你必須理解你的算法。
組合來(lái)自不同傳感器信息的技術(shù)稱之為傳感器融合技術(shù)。之所以較早的討論這個(gè)，是因?yàn)閼?yīng)用在傳感器融合之上的算法必須處理短暫的，充滿雜訊的輸入，生成可靠的運(yùn)動(dòng)狀態(tài)估計(jì)的概率。
下面我們將展示在位置的追蹤和估計(jì)中最通用的算法，卡爾曼濾波器的變種——‘?dāng)U展卡爾曼濾波器’。
1.使用激光雷達(dá)數(shù)據(jù)的基礎(chǔ)卡爾曼濾波器：
卡爾曼濾波器的歷史已經(jīng)超過(guò)半個(gè)世紀(jì)，但是對(duì)于輸入數(shù)據(jù)的噪聲信息和狀態(tài)估計(jì)的平滑來(lái)說(shuō)仍然是最有效的傳感器融合算法之一。它假定位置參數(shù)是符合高斯分布的，即完全可以被均值和協(xié)方差參數(shù)化:X～N(μ, σ2)
當(dāng)傳感器的信息流開(kāi)始的時(shí)候,卡爾曼濾波器使用一系列的狀態(tài)信息來(lái)預(yù)測(cè)和衡量更新步驟去更新被追蹤目標(biāo)的信心值(概率)。預(yù)測(cè)和更細(xì)心的步驟如下圖所示：
狀態(tài)預(yù)測(cè)：
我們會(huì)使用一個(gè)簡(jiǎn)化的線性狀態(tài)空間模型(鏈接：去闡述濾波器的工作方式. 一個(gè)系統(tǒng)在t時(shí)刻的線性狀態(tài)可以從t-1時(shí)刻根據(jù)以下等式被估計(jì)：
衡量更新：
卡爾曼濾波器的下一部分則是去使用實(shí)測(cè)參數(shù)z去更新預(yù)測(cè)狀態(tài)'x',通過(guò)縮放因子(通常稱之為卡爾曼增益)成比例的計(jì)算估計(jì)值和測(cè)量值之間的誤差。
你可以在以下鏈接中找到衡量更新等式的求導(dǎo)：
舉個(gè)例子
理論部分結(jié)束！讓我們嘗試用一些代碼去表示一些卡爾曼濾波器的基礎(chǔ)過(guò)程。
對(duì)此，我們模擬一個(gè)目標(biāo)，它的狀態(tài)被四維向量x=[px py vx vy]所描述。
在這個(gè)例子中測(cè)量傳感器是返回位置數(shù)據(jù)但是沒(méi)有速度信息的激光傳感器。為了觀測(cè)到速度信息我們需要使用雷達(dá)傳感器數(shù)據(jù)。在接下來(lái)的章節(jié)中，當(dāng)我們討論擴(kuò)展卡爾曼濾波的時(shí)候我們將會(huì)涉及到這些。
讓我們以一些假設(shè)開(kāi)始：
偽代碼：
基礎(chǔ)版本的卡爾曼濾波器代碼步驟列在了下面。你可以找到一個(gè)基礎(chǔ)的例子：
最后的迭代步驟通過(guò)測(cè)量和應(yīng)用預(yù)測(cè)以及濾波器的更新步驟如下所示：
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ā)生了哪些變化。紅圈表示初始過(guò)程不確定性。隨著預(yù)測(cè)和測(cè)量更新，我們開(kāi)始有了較小的誤差（半徑），狀態(tài)估計(jì)開(kāi)始越來(lái)越精確。
如同你看到的，最終估計(jì)的狀態(tài)向量x[11.99,2.05]非常接近最終的觀測(cè)值，誤差最小值也縮小到了0.05。
2.擴(kuò)展卡爾曼濾波器——使用雷達(dá)數(shù)據(jù)
雷達(dá)數(shù)據(jù)帶來(lái)了一些更困難的挑戰(zhàn)。雷達(dá)返回?cái)?shù)據(jù)值的是基于極坐標(biāo)系，其由三個(gè)部分構(gòu)成：
- ρ /Range(從原點(diǎn)到此的距離)
- ? / bearing ( ρ 和 x的夾角),
- ρ˙：接近率/距離變化率
由于沒(méi)有H矩陣將狀態(tài)向量映射到雷達(dá)的測(cè)量空間，我們需要一個(gè)函數(shù)h（x）來(lái)將狀態(tài)空間銀蛇到測(cè)量空間以測(cè)量更新步驟。這個(gè)方法是映射極坐標(biāo)到笛卡爾坐標(biāo)的方法的衍生方法，定義如下：
這個(gè)映射闡述了一個(gè)非線性函數(shù)，它將使得卡爾曼濾波器的過(guò)程和測(cè)量符合高斯分布的假設(shè)無(wú)效。擴(kuò)展卡爾曼濾波器使用局部線性模型來(lái)逼近非線性模型，然后使用卡爾曼濾波應(yīng)用到逼近值上。局部線性逼近是通過(guò)計(jì)算當(dāng)前狀態(tài)估計(jì)的一階泰勒展開(kāi)得出的。一階的逼近也叫雅克比矩陣。我們不會(huì)在這里過(guò)多介紹雅克比矩陣的推到過(guò)程。相關(guān)內(nèi)容在網(wǎng)絡(luò)中可以找到優(yōu)秀的介紹，但是如果你想要直接使用這些東西，你可以在以下的github引用中找到代碼實(shí)例：Github link
現(xiàn)實(shí)參考
你可以在github存儲(chǔ)庫(kù)中找到關(guān)于Kalman過(guò)濾器的c++編寫代碼:
代碼示例：
來(lái)源：牛喀網(wǎng)
卡爾曼濾波傳感器融合：【傳感器融合】擴(kuò)展卡爾曼濾波的逐步理解與實(shí)現(xiàn)（上）
本篇文章借鑒了加拿大勞里埃大學(xué)的Simon D. Levy 教授博客對(duì)擴(kuò)展卡爾曼濾波的理解思路。謹(jǐn)慎地對(duì)其中一些冗余的基礎(chǔ)內(nèi)容進(jìn)行刪減，并按照自己的理解進(jìn)行增添改寫。主要是為了自己以后查閱，原博客是很有意思的交互式頁(yè)面，建議有英文閱讀能力的同仁閱讀原博主的文章：Kalman Tutorial - Simon D. Levy
1.一個(gè)基本模型1.1 機(jī)器人小M
為了逐步解釋擴(kuò)展卡爾曼濾波的原理，我們先建立一個(gè)最基本的機(jī)器人小M，它只能在直線上做 方向的一維運(yùn)動(dòng)，如圖所示：
1.2 位移狀態(tài)預(yù)測(cè)值
我們用如下規(guī)律預(yù)測(cè)機(jī)器人的運(yùn)動(dòng)：其當(dāng)前時(shí)刻的位移 是上一時(shí)刻位移 的 倍。即位移滿足如下遞推公式：
這個(gè)公式很好理解，假如小M的初始位移 ，那么第 時(shí)刻的位移就依次為 ，是一條關(guān)于時(shí)刻的指數(shù)曲線。
但實(shí)際上，小M的運(yùn)動(dòng)總是存在各種誤差，這種誤差來(lái)源于機(jī)器人運(yùn)動(dòng)預(yù)測(cè)模型本身的不精確，并不是觀測(cè)引起的（即使用絕對(duì)精確的傳感器進(jìn)行觀測(cè)，這種誤差依然存在）。我們用估計(jì)值 來(lái)替代上式中的精確值 。另外，運(yùn)動(dòng)過(guò)程必然存在噪聲，用 表示：
一般來(lái)說(shuō)，我們將運(yùn)動(dòng)噪聲視為一個(gè)均值為 ，方差為 的高斯分布。因此我們?cè)诮酉聛?lái)的討論中忽略 ，將上式寫為：
（1）式說(shuō)明，如果僅僅依靠運(yùn)動(dòng)模型進(jìn)行預(yù)測(cè)，那么每一步都會(huì)得到含有誤差的估計(jì)值。我們用 表示第 時(shí)刻的預(yù)測(cè)誤差。則顯然預(yù)測(cè)誤差和運(yùn)動(dòng)模型一樣，也呈不精確的遞推關(guān)系：
可以注意到，預(yù)測(cè)誤差的遞推式中，系數(shù)變成了 ，這是因?yàn)槲覀儾捎昧似椒讲顏?lái)表示誤差，其中的邏輯后面會(huì)講到。 是過(guò)程噪聲的平方差，前面已經(jīng)說(shuō)到了。
從上面兩式可以看出，每一步遞推得到的狀態(tài)值和預(yù)測(cè)誤差都是不準(zhǔn)確的估計(jì)值。如果每一步都只包含以上計(jì)算，必然會(huì)導(dǎo)致誤差累積。為了修正這種誤差，我們就要引入傳感器測(cè)量值。
1.3 傳感器測(cè)量值
假設(shè)我們安裝了一個(gè)傳感器，用以測(cè)量小M的精確位移 。我們用 表示 時(shí)刻傳感器所測(cè)量到的位移。理想情況下，傳感器測(cè)到的值就是真實(shí)的位移 。但實(shí)際上，由于傳感器精度有限，我們得到的測(cè)量值總是含有部分噪聲。我們用 表示傳感器噪聲。則傳感器測(cè)量值可以表示如下：
改寫上式，我們可以用傳感器測(cè)量值和噪聲得到位移狀態(tài)：
按道理來(lái)說(shuō)，有這個(gè)公式，我們就可以計(jì)算狀態(tài)準(zhǔn)確值 了。但實(shí)際上，傳感器的噪聲 是未知的（雖然從廠家樣本中包含噪聲的分布狀況，但無(wú)法獲知某一時(shí)刻精確的噪聲值）。因此我們只能另辟蹊徑，采用其它方法以避開(kāi)精確的傳感器噪聲。
1.4 狀態(tài)準(zhǔn)確值 的另一種計(jì)算方法
我們注意到 1.2 中已經(jīng)得到了一個(gè)位移狀態(tài)的估計(jì)值 。而 1.3 中得到了傳感器的測(cè)量值 。我們將二者結(jié)合起來(lái)：
這個(gè)公式的含義是：利用運(yùn)動(dòng)模型所得到的估計(jì)值 和當(dāng)前時(shí)刻的傳感器測(cè)量值 來(lái)計(jì)算當(dāng)前時(shí)刻的準(zhǔn)確狀態(tài)值 。其中，我們引入了一個(gè)增益系數(shù) 來(lái)表示本次估計(jì)的采信偏向。
增益系數(shù)
為了理解公式（3）中增益系數(shù)的含義，我們可以將增益系數(shù) 在 到 之間移動(dòng)。可以發(fā)現(xiàn)，當(dāng)增益系數(shù)等于 時(shí)，當(dāng)前時(shí)刻的狀態(tài)準(zhǔn)確值 等于估計(jì)值 。反之，若增益系數(shù)等于 ，狀態(tài)準(zhǔn)確值 等于傳感器測(cè)量值 。因此，增益系數(shù)的大小實(shí)際上表示了對(duì)運(yùn)動(dòng)模型估計(jì)值和傳感器測(cè)量值的采信程度(更相信哪一方)。
如何確定增益系數(shù)呢？可以做一個(gè)感性的理解：既然增益系數(shù)代表了對(duì)傳感器測(cè)量值的采信程度。那么顯然，如果傳感器噪聲很大，傳感器測(cè)量值的可信程度就很小。雖然我們無(wú)法獲知傳感器噪聲的精確值，但廠家所提供的產(chǎn)品樣板中通常會(huì)包含傳感器噪聲的分布狀況，例如給出了噪聲的方差 。則增益系數(shù)可以通過(guò)如下公式確定：
分析上式，如果傳感器噪聲方差 越大，則增益系數(shù) 越小，代表傳感器測(cè)量值越不可信，邏輯上沒(méi)有問(wèn)題。 的含義在1.2中已經(jīng)說(shuō)過(guò)了，是本次預(yù)測(cè)誤差的估計(jì)值。
我們通過(guò)公式（3）已經(jīng)得到了狀態(tài)的準(zhǔn)確值 ，遞推公式（1）已經(jīng)可以成功運(yùn)行了。但遞推公式（2）中還缺少了預(yù)測(cè)誤差的準(zhǔn)確值 有待計(jì)算。
1.5 預(yù)測(cè)誤差準(zhǔn)確值 的計(jì)算方法
我們已經(jīng)知道了預(yù)測(cè)誤差的估計(jì)值 ，利用增益系數(shù) 可以計(jì)算預(yù)測(cè)誤差準(zhǔn)確值：
分析上式，當(dāng)增益系數(shù) 時(shí)，預(yù)測(cè)誤差的準(zhǔn)確值就等于估計(jì)值 ，這個(gè)邏輯是合理的，因?yàn)楫?dāng)增益系數(shù)為0時(shí)，我們的狀態(tài)準(zhǔn)確值也是直接采信了狀態(tài)估計(jì)值，說(shuō)明我們認(rèn)為傳感器誤差太大，直接采信了運(yùn)動(dòng)模型的預(yù)測(cè)結(jié)果；當(dāng)增益系數(shù) 時(shí)，我們的預(yù)測(cè)誤差準(zhǔn)確值將直接歸零，這個(gè)邏輯也是合理的，從公式（4）可以看出，只有當(dāng)噪聲方差為0（傳感器絕對(duì)準(zhǔn)確）時(shí)，增益系數(shù)才可能為1，而且此時(shí)我們將完全采用傳感器的測(cè)量值作為準(zhǔn)確值（公式3）。既然傳感器絕對(duì)準(zhǔn)確，那么本步的預(yù)測(cè)當(dāng)然不存在任何誤差。
當(dāng)然，以上兩類均是不可能出現(xiàn)的極端情況，實(shí)際情況總是介于兩者之間（沒(méi)有絕對(duì)準(zhǔn)確的運(yùn)動(dòng)模型，也沒(méi)有絕對(duì)準(zhǔn)確的傳感器），否則傳感器融合也就失去了意義。
1.6 總結(jié)
我們將上面標(biāo)出序號(hào)的式子重新按照計(jì)算順序排列出來(lái)：
我們把前兩步叫做預(yù)測(cè)，后三步叫做更新。所謂預(yù)測(cè)，就是利用上一步的狀態(tài)準(zhǔn)確值和預(yù)測(cè)誤差準(zhǔn)確值，結(jié)合運(yùn)動(dòng)模型得到下一步的狀態(tài)估計(jì)值和預(yù)測(cè)誤差估計(jì)值；所謂更新，就是融入傳感器信息，計(jì)算增益系數(shù)，將估計(jì)值再次轉(zhuǎn)換為準(zhǔn)確值。
至此，我們已經(jīng)利用一個(gè)基本模型小M理解了卡爾曼濾波的雛形。雖然真正的卡爾曼濾波形式與此有一些區(qū)別，但基本的邏輯是一致的。接下來(lái)我們逐步完善這一公式。
2 推廣到線性卡爾曼濾波
上面的基本模型和線性卡爾曼濾波的標(biāo)準(zhǔn)模型還有一定的差異，接下來(lái)我們分步填補(bǔ)這些差異，得到真正的線性卡爾曼濾波模型。
2.1 加入控制量
首先我們看狀態(tài)估計(jì)值 的遞推方程：
在這個(gè)遞推方程中，下一時(shí)刻的位移狀態(tài)完全取決于上一時(shí)刻的位移狀態(tài)（和系數(shù) 的乘積）。但實(shí)際上，一個(gè)機(jī)器人的狀態(tài)變化往往不會(huì)這么簡(jiǎn)單，我們通常會(huì)對(duì)機(jī)器人施加一個(gè)人為的影響，以改變其運(yùn)動(dòng)過(guò)程（例如對(duì)機(jī)器人施加一個(gè)外力推動(dòng)其前進(jìn)）。這種人為的影響無(wú)法直接用狀態(tài)量來(lái)表達(dá)，因此我們需要額外引入控制量 及其增益 ，改寫狀態(tài)方程如下：
上式表示，某一時(shí)刻的狀態(tài)估計(jì)值由上一時(shí)刻的狀態(tài)準(zhǔn)確值與控制量共同決定。
2.2 引入觀測(cè)系數(shù)
在基本模型中，我們假設(shè)傳感器的測(cè)量值 在絕對(duì)精確的時(shí)候，就等于狀態(tài)準(zhǔn)確值 。但實(shí)際上，我們的傳感器往往并不會(huì)直接測(cè)量機(jī)器人的狀態(tài)量，而需要經(jīng)過(guò)一個(gè)變換，我們用乘上一個(gè)觀測(cè)系數(shù) 來(lái)表示：
其中 仍然為噪聲精確值。為了理解變換系數(shù) 的含義，我們還是拿小M舉例。假如我們的傳感器是一臺(tái)放在終點(diǎn)的位移傳感器，其正向與小M的位移正向剛好相反，那么其測(cè)得的測(cè)量值就是真實(shí)位移的相反數(shù)，此時(shí) ，如圖所示：
又或者，我們用一個(gè)碼盤來(lái)測(cè)量履帶所轉(zhuǎn)的圈數(shù)，那碼盤的測(cè)量值就是真實(shí)位移除以履帶的周長(zhǎng)，即 ，其中 為履帶周長(zhǎng)，如圖所示：
引入觀測(cè)系數(shù) 以后，整個(gè)更新過(guò)程的形式修改如下：
我們可能會(huì)疑惑其中的 是按照什么邏輯插入的，但只要深入分析就能理解其合理性。還是按照極端情況分析，假設(shè)傳感器噪聲 ，那么此時(shí)增益系數(shù) ，代入公式第二行中，得到 ，說(shuō)明由于傳感器噪聲為0，是絕對(duì)精確的，因此狀態(tài)準(zhǔn)確值 直接采信了傳感器測(cè)量值 ，且其間存在一個(gè)觀測(cè)系數(shù) 的轉(zhuǎn)換關(guān)系；若將增益系數(shù)代入第三行，得到 ，說(shuō)明由于傳感器絕對(duì)精確，且直接采信了傳感器測(cè)量值，因此該步驟的預(yù)測(cè)誤差歸零。以上邏輯全部與基本模型中所推導(dǎo)的一致，說(shuō)明公式（8）的修改是合理的。
2.3 擴(kuò)展到多維
現(xiàn)在，我們的公式已經(jīng)修改為了如下形式：
在真實(shí)的機(jī)器人中，我們所需要的狀態(tài)量往往不止一個(gè)。例如對(duì)小M，我們可能還需要描述其速度 。于是，我們先將狀態(tài)從一個(gè)維度擴(kuò)展到多個(gè)維度，用狀態(tài)向量 來(lái)表示：
為了將公式（9）完整地多維化，我們需要設(shè)計(jì)各個(gè)系數(shù)和變量的多維形式。
第一行：狀態(tài)向量預(yù)測(cè)
由于引入了多維狀態(tài)向量，因此 和 不再代表簡(jiǎn)單的位移遞推關(guān)系，而要代表各個(gè)狀態(tài)分量之間的基于運(yùn)動(dòng)模型的計(jì)算關(guān)系。對(duì)小M來(lái)說(shuō)，就是要代表本時(shí)刻的位移和速度，是如何通過(guò)上一次時(shí)刻的位移和速度（以及控制量）計(jì)算得到的。顯然，這種多維的計(jì)算關(guān)系也不能用一個(gè)變量來(lái)表示，而要將 轉(zhuǎn)換為矩陣。
乍一看很復(fù)雜，好在對(duì)小M來(lái)說(shuō)，這種計(jì)算關(guān)系是比較容易找到的，就是勻加速直線運(yùn)動(dòng)的位移和速度計(jì)算公式：
利用線性代數(shù)知識(shí)，我們很容易將上式寫為矩陣形式：
對(duì)照上式與公式（9）的第一行，我們很容易看出其間的一一對(duì)應(yīng)關(guān)系。因此第一行的多維形式如下：
第二行：誤差預(yù)測(cè)
1.2 節(jié)中提到了，我們用平方差來(lái)表示預(yù)測(cè)誤差。在多維情況下，我們用協(xié)方差矩陣來(lái)替代方差變量，其中主對(duì)角線上的值依次代表每個(gè)狀態(tài)變量本身的誤差（用方差表示），副對(duì)角線代表一個(gè)狀態(tài)變量的誤差隨著另一個(gè)狀態(tài)變量的誤差的變化，一般為0。例如我們小M的位移誤差為0.1，速度誤差也為0.1，則協(xié)方差矩陣為：
關(guān)于更詳細(xì)的協(xié)方差矩陣定義請(qǐng)自行查找資料。
因此第二行可改寫為：
其中 為狀態(tài)變量的協(xié)方差矩陣， 為噪聲的協(xié)方差矩陣。
為 的矩陣乘法形式。
第三到五行：更新
按照上述邏輯，我們可以把更新過(guò)程全部多維化。只需要將各變量改為向量形式，系數(shù)改為矩陣形式，常數(shù)1改為單位矩陣 ，數(shù)的倒數(shù)改為矩陣的逆即可。
將公式（9）完全轉(zhuǎn)化為多維形式，我們就得到了完整的線性卡爾曼濾波公式，如下：
真誠(chéng)說(shuō)明
接下來(lái)我們要做的是將卡爾曼濾波公式進(jìn)行非線性化，得到擴(kuò)展卡爾曼濾波公式，并通過(guò)一個(gè)C++程序來(lái)實(shí)現(xiàn)。本來(lái)想一篇文章搞定，但知乎編輯器寫太多以后變得十分卡頓，體驗(yàn)很惡劣，因此我先把線性部分發(fā)出來(lái)，剩下的盡快補(bǔ)上。
我很努力地按照原博主的文章和自己的理解盡可能地寫清楚，但原博主的文章本身存在一些缺陷，而我自己對(duì)卡爾曼濾波的理解也相當(dāng)有限，所以文章中必然存在一些不足之處。如果發(fā)現(xiàn)了，歡迎大家友善地指出~
卡爾曼濾波傳感器融合：卡爾曼濾波(Kalman Filter)：最好的融合算法之一「AI核心算法」
關(guān)注：決策智能與機(jī)器學(xué)習(xí)，深耕AI脫水干貨
作者：vincentqin ?來(lái)源：計(jì)算機(jī)視覺(jué)SLAM
轉(zhuǎn)載請(qǐng)聯(lián)系作者
譯者注：這恐怕是全網(wǎng)有關(guān)卡爾曼濾波最簡(jiǎn)單易懂的解釋，如果你認(rèn)真的讀完本文，你將對(duì)卡爾曼濾波有一個(gè)更加清晰的認(rèn)識(shí)，并且可以手推卡爾曼濾波。原文作者使用了漂亮的圖片和顏色來(lái)闡明它的原理（讀起來(lái)并不會(huì)因公式多而感到枯燥），所以請(qǐng)勇敢地讀下去！
背景
關(guān)于濾波
首先援引來(lái)自知乎大神的解釋。
“一位專業(yè)課的教授給我們上課的時(shí)候，曾談到：filtering is weighting（濾波即加權(quán)）。濾波的作用就是給不同的信號(hào)分量不同的權(quán)重。最簡(jiǎn)單的loss pass filter， 就是直接把低頻的信號(hào)給1權(quán)重，而給高頻部分0權(quán)重。對(duì)于更復(fù)雜的濾波，比如維納濾波, 則要根據(jù)信號(hào)的統(tǒng)計(jì)知識(shí)來(lái)設(shè)計(jì)權(quán)重。
從統(tǒng)計(jì)信號(hào)處理的角度，降噪可以看成濾波的一種。降噪的目的在于突出信號(hào)本身而抑制噪聲影響。從這個(gè)角度，降噪就是給信號(hào)一個(gè)高的權(quán)重而給噪聲一個(gè)低的權(quán)重。維納濾波就是一個(gè)典型的降噪濾波器。”
關(guān)于卡爾曼濾波
Kalman Filter 算法，是一種遞推預(yù)測(cè)濾波算法，算法中涉及到濾波，也涉及到對(duì)下一時(shí)刻數(shù)據(jù)的預(yù)測(cè)。Kalman Filter 由一系列遞歸數(shù)學(xué)公式描述。它提供了一種高效可計(jì)算的方法來(lái)估計(jì)過(guò)程的狀態(tài)，并使估計(jì)均方誤差最小。卡爾曼濾波器應(yīng)用廣泛且功能強(qiáng)大：它可以估計(jì)信號(hào)的過(guò)去和當(dāng)前狀態(tài)，甚至能估計(jì)將來(lái)的狀態(tài)，即使并不知道模型的確切性質(zhì)。
Kalman Filter 也可以被認(rèn)為是一種數(shù)據(jù)融合算法（Data fusion algorithm），已有50多年的歷史，是當(dāng)今使用最重要和最常見(jiàn)的數(shù)據(jù)融合算法之一。Kalman Filter 的巨大成功歸功于其小的計(jì)算需求，優(yōu)雅的遞歸屬性以及作為具有高斯誤差統(tǒng)計(jì)的一維線性系統(tǒng)的最優(yōu)估計(jì)器的狀態(tài)。
Kalman Filter 只能減小均值為0的測(cè)量噪聲帶來(lái)的影響。只要噪聲期望為0，那么不管方差多大，只要迭代次數(shù)足夠多，那效果都很好。反之，噪聲期望不為0，那么估計(jì)值就還是與實(shí)際值有偏差[3]。
問(wèn)題描述
上面的描述可能把大家繞暈了，下面來(lái)點(diǎn)輕松的。
我們會(huì)有一個(gè)疑問(wèn)：卡爾曼濾波到底是如何解決實(shí)際問(wèn)題的呢？
我們以機(jī)器人為例介紹卡爾曼濾波的原理，我們的任務(wù)是要預(yù)測(cè)機(jī)器人的狀態(tài)
，包括位置
與速度
，即可表示為：
某個(gè)時(shí)刻，我們不知道真實(shí)的位置與速度到底是多少，二者存在一個(gè)所有可能性的組合，大致如下圖所示。
卡爾曼濾波假設(shè)狀態(tài)所有的變量都是隨機(jī)的且都服從高斯分布，每個(gè)變量都有其對(duì)應(yīng)的均值
以及方差
（它代表了不確定性）。
在上圖中，位置和速度是不相關(guān)（uncorrelated）的，這意味著某個(gè)變量的狀態(tài)不會(huì)告訴你其他變量的狀態(tài)是怎樣的。即，我們雖然知道現(xiàn)在的速度，但無(wú)法從現(xiàn)在的速度推測(cè)出現(xiàn)在的位置。但實(shí)際上并非如此，我們知道速度和位置是有關(guān)系的（correlated），這樣一來(lái)二者之間的組合關(guān)系變成了如下圖所示的情況。
這種情況是很容易發(fā)生的，例如，如果速度很快，我們可能會(huì)走得更遠(yuǎn)，所以我們的位置會(huì)更大。如果我們走得很慢，我們就不會(huì)走得太遠(yuǎn)。
這種狀態(tài)變量之間的關(guān)系很重要，因?yàn)樗梢詾槲覀兲峁└嘈畔ⅲ篛ne measurement tells us something about what the others could be。這就是卡爾曼濾波器的目標(biāo)，我們希望從不確定的測(cè)量中盡可能多地獲取信息！
這種狀態(tài)量的相關(guān)性可以由協(xié)方差矩陣表示。簡(jiǎn)而言之，矩陣
的每個(gè)元素是第i個(gè)狀態(tài)變量和第j個(gè)狀態(tài)變量之間的相關(guān)度。（顯然地可以知道協(xié)方差矩陣是對(duì)稱的，這意味著交換i和j都沒(méi)關(guān)系）。協(xié)方差矩陣通常標(biāo)記為“?
”，因此我們將它們的元素稱為“
”。
狀態(tài)預(yù)測(cè)
問(wèn)題的矩陣形式表示
我們把狀態(tài)建模成高斯分布（Gaussian blob，由于二維高斯分布長(zhǎng)得像一個(gè)個(gè)小泡泡，之所以長(zhǎng)這個(gè)樣子，可參考鏈接[2]）。我們需要求解/估計(jì)在時(shí)間
時(shí)刻的兩個(gè)信息：1. 最優(yōu)估計(jì)
以及它的協(xié)方差矩陣
，我們可以寫成下面矩陣形式：
（當(dāng)然，這里我們僅使用位置和速度，但是請(qǐng)記住狀態(tài)可以包含任意數(shù)量的變量，并且可以表示所需的任何變量）
接下來(lái)，我們需要某種方式來(lái)查看當(dāng)前狀態(tài)（
時(shí)刻）并預(yù)測(cè)在時(shí)刻
處的狀態(tài)。請(qǐng)記住，我們不知道哪個(gè)狀態(tài)是“真實(shí)”狀態(tài)，但是這里提到的預(yù)測(cè)（prediction）并不在乎這些。
我們可以用一個(gè)矩陣
來(lái)表示這個(gè)預(yù)測(cè)過(guò)程：
這個(gè)矩陣
將原始估計(jì)中的每個(gè)點(diǎn)移動(dòng)到新的預(yù)測(cè)位置。
那么問(wèn)題來(lái)了，應(yīng)該如何使用上述矩陣來(lái)預(yù)測(cè)下一時(shí)刻的位置和速度呢？為了闡述這個(gè)過(guò)程，我們使用了一個(gè)非常基礎(chǔ)的運(yùn)動(dòng)學(xué)公式（初中物理中就學(xué)過(guò)）進(jìn)行描述：
寫成矩陣形式：
現(xiàn)在我們有了一個(gè)預(yù)測(cè)矩陣或者叫做狀態(tài)轉(zhuǎn)移矩陣，該矩陣可以幫助我們計(jì)算下一個(gè)時(shí)刻的狀態(tài)。但我們?nèi)匀徊恢廊绾胃聽(tīng)顟B(tài)的協(xié)方差矩陣，其實(shí)過(guò)程也是很簡(jiǎn)單，如果我們將分布中的每個(gè)點(diǎn)乘以矩陣
，那么其協(xié)方差矩陣
會(huì)發(fā)生什么？
將公式(3)代入公式(4)我們可以得到：
External influence
不過(guò)我們并沒(méi)有考慮到所有的影響因素。可能有一些與狀態(tài)本身無(wú)關(guān)的變化——如外界因素可能正在影響系統(tǒng)。
例如，我們用狀態(tài)對(duì)列車的運(yùn)動(dòng)進(jìn)行建模，如果列車長(zhǎng)加大油門，火車就加速。同樣，在我們的機(jī)器人示例中，導(dǎo)航系統(tǒng)軟件可能會(huì)發(fā)出使車輪轉(zhuǎn)動(dòng)或停止的命令。如果我們很明確地知道這些因素，我們可以將其放在一起構(gòu)成一個(gè)向量
，我們可以對(duì)這個(gè)量進(jìn)行某些“處理”，然后將其添加到我們的預(yù)測(cè)中對(duì)狀態(tài)進(jìn)行更正。
假設(shè)我們知道由于油門設(shè)置或控制命令而產(chǎn)生的預(yù)期加速度
。根據(jù)基本運(yùn)動(dòng)學(xué)原理，我們可以得到下式：
將其寫成矩陣形式：
其中
被稱為控制矩陣，
被稱為控制向量。（注意：對(duì)于沒(méi)有外部影響的簡(jiǎn)單系統(tǒng)，可以忽略這個(gè)控制項(xiàng)）。
如果我們的預(yù)測(cè)并不是100％準(zhǔn)確模型，這會(huì)發(fā)生什么呢？
External uncertainty
如果狀態(tài)僅僅依賴其自身的屬性進(jìn)行演進(jìn)，那一切都會(huì)很好。如果狀態(tài)受到外部因素進(jìn)行演進(jìn)，我們只要知道這些外部因素是什么，那么一切仍然很好。
但在實(shí)際使用中，我們有時(shí)不知道的這些外部因素到底是如何被建模的。例如，我們要跟蹤四軸飛行器，它可能會(huì)隨風(fēng)搖晃；如果我們跟蹤的是輪式機(jī)器人，則車輪可能會(huì)打滑，或者因地面顛簸導(dǎo)致其減速。我們無(wú)法跟蹤這些外部因素，如果發(fā)生任何這些情況，我們的預(yù)測(cè)可能會(huì)出錯(cuò)，因?yàn)槲覀儾](méi)有考慮這些因素。
通過(guò)在每個(gè)預(yù)測(cè)步驟之后添加一些新的不確定性，我們可以對(duì)與“世界”相關(guān)的不確定性進(jìn)行建模（如我們無(wú)法跟蹤的事物）：
這樣一來(lái)，由于新增的不確定性原始估計(jì)中的每個(gè)狀態(tài)都可能遷移到多個(gè)狀態(tài)。因?yàn)槲覀兎浅Ｏ矚g用高斯分布進(jìn)行建模，此處也不例外。我們可以說(shuō)
的每個(gè)點(diǎn)都移動(dòng)到具有協(xié)方差
的高斯分布內(nèi)的某個(gè)位置，如下圖所示：
這將產(chǎn)生一個(gè)新的高斯分布，其協(xié)方差不同（但均值相同）：
所以呢，我們?cè)跔顟B(tài)量的協(xié)方差中增加額外的協(xié)方差
，所以預(yù)測(cè)階段完整的狀態(tài)轉(zhuǎn)移方程為：
換句話說(shuō)：新的最佳估計(jì)是根據(jù)先前的最佳估計(jì)做出的預(yù)測(cè)，再加上對(duì)已知外部影響的校正。
新的不確定度是根據(jù)先前的不確定度做出的預(yù)測(cè)，再加上來(lái)自環(huán)境額外的不確定度。
上述過(guò)程描繪了狀態(tài)預(yù)測(cè)過(guò)程，那么當(dāng)我們從傳感器中獲取一些測(cè)量數(shù)據(jù)時(shí)會(huì)發(fā)生什么呢？
狀態(tài)更新
利用測(cè)量進(jìn)一步修正狀態(tài)
假設(shè)我們有幾個(gè)傳感器，這些傳感器可以向我們提供有關(guān)系統(tǒng)狀態(tài)的信息。就目前而言，測(cè)量什么量都無(wú)關(guān)緊要，也許一個(gè)讀取位置，另一個(gè)讀取速度。每個(gè)傳感器都告訴我們有關(guān)狀態(tài)的一些間接信息（換句話說(shuō)，傳感器在狀態(tài)下運(yùn)作并產(chǎn)生一組測(cè)量讀數(shù)）。
請(qǐng)注意，測(cè)量的單位可能與狀態(tài)量的單位不同。我們使用矩陣
對(duì)傳感器的測(cè)量進(jìn)行建模。
所以我們期望傳感器的度數(shù)可以被建模成如下形式：
卡爾曼濾波器的偉大之處就在于它能夠處理傳感器噪聲。換句話說(shuō)，傳感器本身的測(cè)量是不準(zhǔn)確的，且原始估計(jì)中的每個(gè)狀態(tài)都可能導(dǎo)致一定范圍的傳感器讀數(shù)，而卡爾曼濾波能夠在這些不確定性存在的情況下找到最優(yōu)的狀態(tài)。
根據(jù)傳感器的讀數(shù)，我們會(huì)猜測(cè)系統(tǒng)正處于某個(gè)特定狀態(tài)。但是由于不確定性的存在，某些狀態(tài)比其他狀態(tài)更可能產(chǎn)生我們看到的讀數(shù)：
我們將這種不確定性（如傳感器噪聲）的協(xié)方差表示為
，讀數(shù)的分布均值等于我們觀察到傳感器的讀數(shù)，我們將其表示為
這樣一來(lái)，我們有了兩個(gè)高斯分布：一個(gè)圍繞通過(guò)狀態(tài)轉(zhuǎn)移預(yù)測(cè)的平均值，另一個(gè)圍繞實(shí)際傳感器讀數(shù)。
因此，我們需要將基于預(yù)測(cè)狀態(tài)（粉紅色）的推測(cè)讀數(shù)與基于實(shí)際觀察到的傳感器讀數(shù)（綠色）進(jìn)行融合。
那么融合后最有可能的新?tīng)顟B(tài)是什么？對(duì)于任何可能的讀數(shù)
，我們都有兩個(gè)相關(guān)的概率：（1）我們的傳感器讀數(shù)
是
的測(cè)量值的概率，以及（2）先前估計(jì)值的概率認(rèn)為
是我們應(yīng)該看到的讀數(shù)。
如果我們有兩個(gè)概率，并且想知道兩個(gè)概率都為真的機(jī)會(huì)，則將它們相乘。因此，我們對(duì)兩個(gè)高斯分布進(jìn)行了相乘處理：
兩個(gè)概率分布相乘得到的就是上圖中的重疊部分。而且重疊部分的概率分布會(huì)比我們之前的任何一個(gè)估計(jì)值/讀數(shù)都精確得多，這個(gè)分布的均值就是兩種估計(jì)最有可能配置（得到的狀態(tài)）。
事實(shí)證明，兩個(gè)獨(dú)立的高斯分布相乘之后會(huì)得到一個(gè)新的具有其均值和協(xié)方差矩陣的高斯分布！下面開(kāi)始推公式。
合并兩個(gè)高斯分布
首先考慮一維高斯情況：一個(gè)均值為
，方差為
的高斯分布的形式為：
我們想知道將兩個(gè)高斯曲線相乘會(huì)發(fā)生什么。下圖中的藍(lán)色曲線表示兩個(gè)高斯總體的（未歸一化）交集：
將公式(9)代入公式(10)，我們可以得到新的高斯分布的均值和方差如下所示：
我們將其中的一小部分重寫為
：
這樣一來(lái)，公式的形式就簡(jiǎn)單多了！我們順勢(shì)將公式(12)和(13)的矩陣形式寫在下面：
其中
表示新高斯分布的協(xié)方差矩陣，
是每個(gè)維度的均值，
就是大名鼎鼎的“卡爾曼增益”（Kalman gain）。
公式匯總
我們有兩個(gè)高斯分布，一個(gè)是我們預(yù)測(cè)的觀測(cè)，另外一個(gè)是實(shí)際的觀測(cè)(傳感器讀數(shù))
，我們將這兩個(gè)高斯分布帶入公式(15)中就可以得到二者的重疊區(qū)域：
我們有兩個(gè)高斯分布，一個(gè)是我們預(yù)測(cè)的觀測(cè)，另外一個(gè)是實(shí)際的觀測(cè)(傳感器讀數(shù))
，我們將這兩個(gè)高斯分布帶入公式(15)中就可以得到二者的重疊區(qū)域：
從公式(14)我們可以知道，卡爾曼增益是：
然后我們將公式(16)與公式(17)中的
去除，同時(shí)將
后面的
去除，我們可以得到最終的化簡(jiǎn)形式的更新方程：
圖說(shuō)
大功告成，
就是更新后的最優(yōu)狀態(tài)！接下來(lái)我們可以繼續(xù)進(jìn)行預(yù)測(cè)，然后更新，重復(fù)上述過(guò)程！下圖給出卡爾曼濾波信息流。
總結(jié)
在上述所有數(shù)學(xué)公式中，你需要實(shí)現(xiàn)的只是公式(7)(18)和(19)。（或者，如果你忘記了這些，可以從等式(4)和(15)重新推導(dǎo)所有內(nèi)容。）
這將使你能夠準(zhǔn)確地對(duì)任何線性系統(tǒng)建模。對(duì)于非線性系統(tǒng)，我們使用擴(kuò)展卡爾曼濾波器，該濾波器通過(guò)簡(jiǎn)單地線性化預(yù)測(cè)和測(cè)量值的均值進(jìn)行工作。
參考資料
[1]: How a Kalman filter works, in pictures, 圖解卡爾曼濾波是如何工作的：
?
[2]: A geometric interpretation of the covariance matrix, 協(xié)方差矩陣的幾何解釋:?
[3]: Kalman Filter 卡爾曼濾波:?
歷史精華好文
專輯1：AI工程落地
專輯2：AI核心算法
專輯3：AI優(yōu)質(zhì)資源
交流合作
請(qǐng)加微信號(hào)：yan_kylin_phenix，注明姓名+單位+從業(yè)方向+地點(diǎn)，非誠(chéng)勿擾。
卡爾曼濾波傳感器融合：卡爾曼濾波器是如何運(yùn)用于多傳感器融合的？
我目前認(rèn)為的卡爾曼濾波器是這樣的：
1.設(shè)置狀態(tài)轉(zhuǎn)移矩陣
2.[預(yù)測(cè)]通過(guò) t-1 時(shí)的狀態(tài)預(yù)測(cè) t 時(shí)的狀態(tài)
3.[更新]用傳感器的測(cè)量值與 2 中的預(yù)測(cè)值估計(jì)出最優(yōu)值，并更新卡爾曼增益等常數(shù)
4.重復(fù) 2～3 步驟
（如上正是opencv庫(kù)中自帶的卡爾曼濾波器例子的步驟，也是初學(xué)者能最直觀理解到的，它只涉及到一個(gè)測(cè)量值）
那么請(qǐng)問(wèn)，在某些涉及多傳感器融合的問(wèn)題中，例如 陀螺儀（累積誤差）和電子羅盤（干擾）的數(shù)據(jù)融合，此時(shí)有多個(gè)測(cè)量值存在，卡爾曼濾波器又是怎樣工作的呢？
貼一個(gè)數(shù)據(jù)融合的結(jié)果，用陀螺儀和GPS數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，濾波估計(jì)車輛前輪轉(zhuǎn)角即行業(yè)內(nèi)所說(shuō)的慣導(dǎo)角位移。LZ精準(zhǔn)農(nóng)業(yè)領(lǐng)域，器件型號(hào)不表，用霍爾傳感器(精度0.1度)標(biāo)定，精度在0.17度，用于農(nóng)機(jī)自動(dòng)駕駛，真他娘的好用。。。。。。
補(bǔ)一個(gè)IMU姿態(tài)測(cè)量系統(tǒng)數(shù)據(jù)融合的結(jié)果。陀螺和加表數(shù)據(jù)融合，濾波估計(jì)系統(tǒng)俯仰角和橫滾角。器件型號(hào)不表。
這個(gè)是實(shí)驗(yàn)室桌面實(shí)驗(yàn)，手動(dòng)翻轉(zhuǎn)IMU
下面是室外實(shí)驗(yàn)，用GPS雙天線的俯仰角進(jìn)行精度標(biāo)定，GPS主副天線基線2米，俯仰角精度0.1度。IMU剛性鏈接在GPS工裝上，兩手托舉行走。
更一個(gè)濾波融合的例子。
霍爾傳感器安裝在車輛輪子處測(cè)量車輛前輪的角度，但是霍爾傳感器測(cè)量的是0到360度的絕對(duì)角度，安裝在車輪轉(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型號(hào)是ADI某較低成本器件，位置速度精度和RTK狀態(tài)下GNSS結(jié)果保持一致，航向精度0.12度(RMS)。
產(chǎn)品是農(nóng)機(jī)自動(dòng)駕駛儀，用來(lái)輔助農(nóng)機(jī)的自動(dòng)駕駛進(jìn)行作業(yè)，目前主要應(yīng)用在插秧機(jī)、拖拉機(jī)、割草機(jī)等四輪小車。
組合航向精度和雙天線GNSS航向精度基本一致，且靜止情況下航向不漂。
農(nóng)機(jī)橫向控制精度<2.5cm RMS。
