與我的朋友交談時(shí)，我經(jīng)常聽到：“哦，卡爾曼(Kalman)濾波器……我經(jīng)常學(xué)它，然后我什么都忘了”。好吧，考慮到卡爾曼濾波器(KF)是世界上應(yīng)用最廣泛的算法之一(如果環(huán)顧四周，你80％的技術(shù)可能已經(jīng)在內(nèi)部運(yùn)行某種KF)，讓我們嘗試將其弄清楚。

在這篇文章的結(jié)尾，你將對(duì)KF的工作原理，其背后的想法，為什么需要多個(gè)變體以及最常見的變體有一個(gè)直觀而詳細(xì)的了解。

狀態(tài)估計(jì)

KF是所謂的狀態(tài)估計(jì)算法的一部分。什么是狀態(tài)估計(jì)？假設(shè)你有一個(gè)系統(tǒng)(讓我們將其視為黑箱)。黑箱可以是任何東西：你的風(fēng)扇，化學(xué)系統(tǒng)，移動(dòng)機(jī)器人。對(duì)于這些系統(tǒng)中的每一個(gè)，我們都可以定義一個(gè)狀態(tài)。狀態(tài)是我們關(guān)心的變量向量，可以描述系統(tǒng)處于特定時(shí)間點(diǎn)的“狀態(tài)”(這就是為什么將其稱為狀態(tài))。“可以描述”是什么意思？這意味著，如果你了解當(dāng)時(shí)的狀態(tài)向量k和提供給系統(tǒng)的輸入，則可以了解當(dāng)時(shí)的k+1的系統(tǒng)狀態(tài)(與此同時(shí)使用系統(tǒng)工作原理的一些知識(shí))。

例如，假設(shè)我們有一個(gè)移動(dòng)的機(jī)器人，并且我們關(guān)心其在空間中的位置(并且不在乎其方向)。如果我們將狀態(tài)定義為機(jī)器人的位置(x, y)及其速度，($vx$, $vy$)并且我們有一個(gè)機(jī)器人如何運(yùn)動(dòng)的模型，那么就足以確定機(jī)器人的位置以及下一個(gè)時(shí)刻的位置。

因此，狀態(tài)估計(jì)算法估計(jì)系統(tǒng)的狀態(tài)。為什么要估算呢？因?yàn)樵诂F(xiàn)實(shí)生活中，外部觀察者永遠(yuǎn)無(wú)法訪問(wèn)系統(tǒng)的真實(shí)狀態(tài)。通常有兩種情況：你可以測(cè)量狀態(tài)，但是測(cè)量結(jié)果會(huì)受到噪聲的影響(每個(gè)傳感器只能產(chǎn)生一定精度的讀數(shù)，可能對(duì)你來(lái)說(shuō)還不夠)，或者你無(wú)法直接測(cè)量狀態(tài)。一個(gè)例子可能是使用GPS計(jì)算上述移動(dòng)機(jī)器人的位置(我們將位置確定為狀態(tài)的一部分)，這可能會(huì)給你帶來(lái)多達(dá)10米的測(cè)量誤差，對(duì)于你可能想到的任何應(yīng)用程序來(lái)說(shuō)，這可能都不夠精確。

通常，當(dāng)你進(jìn)行狀態(tài)估計(jì)時(shí)，你可以放心地假設(shè)你知道系統(tǒng)的輸入(因?yàn)槭悄憬o出的)和輸出。由于測(cè)量了輸出，因此它也會(huì)受到一定的測(cè)量噪聲的影響。據(jù)此，我們將狀態(tài)估計(jì)器定義為一個(gè)系統(tǒng)，該系統(tǒng)接收你要估計(jì)其狀態(tài)的系統(tǒng)的輸入和輸出并輸出系統(tǒng)狀態(tài)的估計(jì)。

傳統(tǒng)上，狀態(tài)用表示x，輸出用y或z，u是輸入，$tilde_x$是估計(jì)狀態(tài)。

卡爾曼濾波器

你可能已經(jīng)注意到，我們已經(jīng)討論了一些有關(guān)誤差的內(nèi)容：

你可以測(cè)量系統(tǒng)的輸出，但是傳感器會(huì)給出測(cè)量誤差

你可以估計(jì)狀態(tài)，但是作為狀態(tài)估計(jì)它具有一定的置信度。

除此之外，我說(shuō)過(guò)，你需要某種系統(tǒng)知識(shí)，你需要了解系統(tǒng)“行為”的模型(稍后會(huì)詳細(xì)介紹)，你的模型當(dāng)然并不完美，因此你將擁有另一個(gè)誤差。

在KF中，你可以使用高斯分布來(lái)處理所有這些不確定性。高斯分布是表示你不確定的事物的一種好方法。你當(dāng)前確定的東西可以用分布的均值表示，而標(biāo)準(zhǔn)差可以說(shuō)明你對(duì)該確定的信心。

在KF中：

你的估計(jì)狀態(tài)將是具有一定均值和協(xié)方差的高斯隨機(jī)變量(它將告訴我們?cè)撍惴ā按_定”其當(dāng)前估計(jì)的程度)

你對(duì)原始系統(tǒng)的輸出度量的不確定性將用均值為0和一定協(xié)方差的隨機(jī)變量表示(這將告訴我們我們對(duì)度量本身的信任程度)

系統(tǒng)模型的不確定性將由均值為0和一定協(xié)方差的隨機(jī)變量表示(這將告訴我們我們對(duì)使用的模型有多信任)。

讓我們舉一些例子來(lái)了解其背后的想法。

不良模型，好的傳感器，讓我們?cè)俅渭僭O(shè)你要跟蹤機(jī)器人的位置，并且你在傳感器上花費(fèi)了很多錢，它們?yōu)槟闾峁├迕准?jí)的精度。另一方面，你根本不喜歡機(jī)器人技術(shù)，搜索了一下，發(fā)現(xiàn)了一個(gè)非?；镜倪\(yùn)動(dòng)模型：隨機(jī)游走(基本上是一個(gè)僅由噪聲給出運(yùn)動(dòng)的粒子)。很明顯，你的模型不是很好，不能真正被信任，而你的測(cè)量結(jié)果卻很好。在這種情況下，你可能將使用非常窄的高斯分布(小方差)來(lái)建模測(cè)量噪聲，而使用非常寬的高斯分布(大方差)來(lái)建模不確定性。

傳感器質(zhì)量差，模型好，如果傳感器質(zhì)量不好(例如GPS)，但是你花費(fèi)大量時(shí)間對(duì)系統(tǒng)進(jìn)行建模，則情況恰好相反。在這種情況下，你可能將使用非常窄的高斯分布(小方差)來(lái)建模模型不確定性，而使用非常寬的高斯分布(大方差)來(lái)建模噪聲。

估計(jì)的狀態(tài)不確定性如何？

KF將根據(jù)估計(jì)過(guò)程中發(fā)生的事情進(jìn)行更新，你唯一要做的就是將其初始化為足夠好的值?！白銐蚝谩比Q于你的應(yīng)用程序，你的傳感器，你的模型等。通常，KF需要一點(diǎn)時(shí)間才能收斂到正確的估計(jì)值。

KF如何工作？

正如我們所說(shuō)，要讓KF正常工作，你需要對(duì)系統(tǒng)有“一定的了解”。特別是對(duì)于KF，你需要兩個(gè)模型：

狀態(tài)轉(zhuǎn)換模型：某些函數(shù)，給定時(shí)間步k的狀態(tài)和輸入，給出時(shí)間步k+1的狀態(tài)。

測(cè)量模型：某個(gè)函數(shù)，給定時(shí)間步k的狀態(tài)，即可為你提供同一時(shí)間的測(cè)量結(jié)果

稍后，我們將了解為什么需要這些功能，讓我們首先查看一些示例以了解它們的含義。

狀態(tài)轉(zhuǎn)換模型

該模型告訴你系統(tǒng)如何隨時(shí)間變化(如果你還記得的話，我們之前曾談到狀態(tài)必須具有足夠的描述性以及時(shí)推斷系統(tǒng)行為)。這在很大程度上取決于系統(tǒng)本身以及你對(duì)系統(tǒng)的關(guān)心。如果你不知道如何對(duì)系統(tǒng)建模，則可以使用一些Google搜索來(lái)提供幫助。對(duì)于運(yùn)動(dòng)的物體(如果以適當(dāng)?shù)牟蓸勇蕼y(cè)量)，可以使用恒速模型(假定物體以恒定的速度運(yùn)動(dòng))，對(duì)于車輛，可以使用單輪腳踏車模型，等等……讓我們假設(shè)一種或另一種方式，我們建立了一個(gè)模型。我們?cè)谶@里做出一個(gè)重要的假設(shè)，這對(duì)于KF的工作是必要的：你的當(dāng)前狀態(tài)僅取決于先例。換句話說(shuō)，系統(tǒng)狀態(tài)的“歷史”會(huì)壓縮為先前的狀態(tài)，也就是說(shuō)，給定先例狀態(tài)，每個(gè)狀態(tài)都獨(dú)立于過(guò)去。這也稱為馬爾可夫假設(shè)。如果這不成立，你將無(wú)法僅根據(jù)先例來(lái)表達(dá)當(dāng)前狀態(tài)。

測(cè)量模型(Measurement model)

測(cè)量模型告訴你如何將輸出和狀態(tài)聯(lián)系在一起。直觀上，你需要這樣做，因?yàn)槟阒罍y(cè)量的輸出，并且想要在估計(jì)期間從中推斷出狀態(tài)。同樣，此模型因情況而異。例如，在移動(dòng)機(jī)器人示例中，如果你的狀態(tài)是位置并且你擁有GPS，則你的模型就是單位函數(shù)(identity function)，因?yàn)槟阋呀?jīng)在知道了狀態(tài)的有噪聲版本。

每個(gè)步驟的數(shù)學(xué)公式和解釋如下：

那么，KF實(shí)際如何運(yùn)作？該算法分兩個(gè)步驟工作，稱為預(yù)測(cè)和更新。假設(shè)我們?cè)跁r(shí)間步k，并且那時(shí)我們具有估計(jì)狀態(tài)。首先，我們使用狀態(tài)轉(zhuǎn)換模型，并使估計(jì)狀態(tài)預(yù)測(cè)到下一個(gè)時(shí)刻。這相當(dāng)于說(shuō)：鑒于我目前對(duì)狀態(tài)的信念，我所擁有的輸入以及對(duì)系統(tǒng)的了解，我希望我的下一個(gè)狀態(tài)是這樣。這是預(yù)測(cè)步驟。

現(xiàn)在，由于我們還具有輸出和測(cè)量模型，因此我們實(shí)際上可以使用實(shí)際測(cè)量“校正”預(yù)測(cè)。在更新步驟中，我們采用預(yù)期狀態(tài)，我們計(jì)算輸出(使用測(cè)量模型)(2)，并將其與實(shí)際測(cè)量的輸出進(jìn)行比較。然后，我們以“智能方式”使用兩者之間的差異來(lái)校正狀態(tài)估計(jì)(3)。

通常，我們?cè)谛Ｕ坝胊pex -表示狀態(tài)的估計(jì)，它來(lái)自于預(yù)測(cè)步驟。K是卡爾曼增益。這就是巧妙之處:K取決于我們對(duì)度量的信任程度，取決于我們對(duì)當(dāng)前估計(jì)的信任程度(這取決于我們對(duì)模型的信任程度)，根據(jù)這些信息，K“決定”預(yù)測(cè)的估計(jì)在多大程度上被測(cè)量糾正。如果我們的測(cè)量噪聲與我們對(duì)來(lái)自預(yù)測(cè)步驟的估計(jì)的信任程度相比是“小”的，我們將使用測(cè)量對(duì)估計(jì)進(jìn)行大的校正，如果相反，我們將對(duì)其進(jìn)行最小程度的校正。

注意：為簡(jiǎn)單起見，我寫方程式時(shí)就好像在處理普通變量一樣，但是你必須考慮到在每一步中我們都在處理隨機(jī)的高斯變量，因此我們還需要通過(guò)函數(shù)傳播變量的協(xié)方差，而不僅僅是均值。

讓我們舉例說(shuō)明。假設(shè)我們正在(再次)跟蹤一個(gè)機(jī)器人的位置。實(shí)際位置顯示為灰色，k時(shí)刻我們認(rèn)為機(jī)器人處于綠色位置，估計(jì)協(xié)方差表示為一個(gè)橢圓。粗略地說(shuō)，你可以從橢圓的形狀看出，我們的過(guò)濾器在這一步對(duì)橫向定位比在向前運(yùn)動(dòng)方向的定位更“自信”。在使用狀態(tài)轉(zhuǎn)換模型讓系統(tǒng)演進(jìn)的預(yù)測(cè)步驟之后，我們認(rèn)為新的位置是紅色的。由于橢圓在橫向上變大了，我們現(xiàn)在對(duì)新的估計(jì)位置不太確定(例如，因?yàn)槲覀儾惶嘈拍Ｐ?。然后我們讀取GPS，得到黑色的位置。在更新步驟中，實(shí)際的位置估計(jì)將是深綠色的虛線部分。如果我們更信任模型(與測(cè)量噪聲協(xié)方差相比，協(xié)方差更小)，估計(jì)值將更接近紅色;如果我們更信任測(cè)量(與模型不確定性相比，噪聲測(cè)量協(xié)方差更小)，估計(jì)值將更接近紅色。

KF家族

根據(jù)所使用的模型類型(狀態(tài)轉(zhuǎn)換和測(cè)量)，可以將KF分為兩個(gè)大類：如果模型是線性的，則具有線性卡爾曼濾波器，而如果它們是非線性的，則具有非線性卡爾曼濾波器。

為什么要區(qū)分？好吧，KF假設(shè)你的變量是高斯變量，當(dāng)通過(guò)線性函數(shù)傳遞時(shí)，高斯變量仍然是高斯變量，如果通過(guò)非線性函數(shù)傳遞，則不正確。這打破了卡爾曼假設(shè)，因此我們需要找到解決方法。

歷史上，人們發(fā)現(xiàn)了兩種主要的方法:利用模型作弊和利用數(shù)據(jù)作弊。如果你在模型上做了弊你基本上是將當(dāng)前估計(jì)周圍的非線性函數(shù)線性化，這樣你就回到了線性的情況下。這種方法稱為擴(kuò)展卡爾曼濾波(EKF)。這種方法的主要缺點(diǎn)是必須能夠計(jì)算f()和h()的雅可比矩陣?；蛘?，如果你在數(shù)據(jù)上作弊，你使用非線性函數(shù)，然后你嘗試“高斯化”(如果這個(gè)詞存在的話)你做出的非高斯分布。這是通過(guò)一種叫做無(wú)損變換的智能采樣技術(shù)實(shí)現(xiàn)的。這個(gè)變換允許你用平均值和協(xié)方差來(lái)描述(近似地)一個(gè)分布(只有高斯分布才能被前兩個(gè)矩完全描述)。這種方法稱為無(wú)損卡爾曼濾波(UKF)。理論上，UKF優(yōu)于EKF，因?yàn)闊o(wú)損變換比線性化模型得到的近似更接近結(jié)果分布。在實(shí)踐中，你必須有相當(dāng)大的非線性才能真正看到大的區(qū)別。

實(shí)際中的KF

由于我談到了很多有關(guān)帶GPS的移動(dòng)機(jī)器人的內(nèi)容，因此我就此情況作了簡(jiǎn)短的演示(如果要使用它，可以在這里找到代碼)。機(jī)器人的運(yùn)動(dòng)是使用單輪模型生成的。用于KF的狀態(tài)轉(zhuǎn)換模型是等速模型，其狀態(tài)包含x和y位置，轉(zhuǎn)向角及其導(dǎo)數(shù)。

機(jī)器人會(huì)及時(shí)移動(dòng)(實(shí)際位置顯示為黑色)，在每個(gè)步驟中，你都會(huì)得到非常嘈雜的GPS測(cè)量值，該測(cè)量值給出x和y(紅色)并估算位置(藍(lán)色)。你可以使用不同的參數(shù)，看看它們?nèi)绾斡绊憼顟B(tài)估計(jì)。如你所見，我們可以進(jìn)行非常嘈雜的測(cè)量，并對(duì)實(shí)際位置進(jìn)行很好的估算。