詳細(xì)了解卡爾曼濾波器

深入研究Kalman濾波器，這是有史以來最廣泛且最有用的算法之一。

與我的朋友交談時(shí)，我經(jīng)常聽到："哦，卡爾曼過濾器……我通常學(xué)習(xí)它們，理解它們，然后我忘記了一切"。 好吧，考慮到卡爾曼濾波器（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)是變量的向量，我們很想知道它們，它們可以描述系統(tǒng)在特定時(shí)間點(diǎn)的"狀態(tài)"（這就是為什么將其稱為狀態(tài)）。 "可以描述"是什么意思？ 這意味著，如果您知道在時(shí)間k處的狀態(tài)向量和提供給系統(tǒng)的輸入，則可以（同時(shí)使用系統(tǒng)工作原理的一些知識(shí)）知道在時(shí)間k + 1處的系統(tǒng)狀態(tài)。

例如，假設(shè)我們有一個(gè)移動(dòng)的機(jī)器人，并且我們關(guān)心知道它在空間中的位置（而不關(guān)心它的方向）。 如果我們將狀態(tài)定義為機(jī)器人的位置（x，y）及其速度（v_x，v_y），并且我們有一個(gè)關(guān)于機(jī)器人如何運(yùn)動(dòng)的模型，那么就足以確定機(jī)器人的位置和位置。 下一次瞬間。

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

通常，在進(jìn)行狀態(tài)估計(jì)時(shí)，您可以放心地假設(shè)您知道系統(tǒng)的輸入（因?yàn)槭悄峁┑模┖洼敵觥?由于測(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)。

System and State Estimator block diagram.

卡爾曼濾波器

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

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

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

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

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

在KF中：

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

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

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

讓我們舉一些例子來了解其背后的想法。

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

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

Wide variance vs. small variance Gaussian distributions.

估計(jì)狀態(tài)不確定性如何？KF將根據(jù)估計(jì)過程中發(fā)生的事情進(jìn)行更新，您唯一要做的就是將其初始化為足夠好的值。 "足夠好"取決于您的應(yīng)用程序，您的傳感器，您的模型等。通常，KF需要一點(diǎn)時(shí)間才能收斂到正確的估計(jì)值。

KF如何運(yùn)作？

如前所述，要讓KF正常工作，您需要對(duì)系統(tǒng)有"一定的了解"（"不確定"，即不完美的模型）。 特別是對(duì)于KF，您需要兩個(gè)模型：

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

· 測(cè)量模型：給定時(shí)間為k的某個(gè)函數(shù)，可以為您提供同一時(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搜索來提供幫助。對(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)僅取決于先例。換句話說，系統(tǒng)狀態(tài)的"歷史"濃縮為先前的狀態(tài)，也就是說，給定先前的狀態(tài)，每個(gè)狀態(tài)都獨(dú)立于過去。這也稱為馬爾可夫假設(shè)。如果這不成立，就不能僅憑先例來表達(dá)當(dāng)前狀態(tài)。

度量模型度量模型告訴您如何將輸出（可以度量）和狀態(tài)聯(lián)系在一起。 直觀上，您需要這樣做，因?yàn)槟罍y(cè)量的輸出，并且想要在估計(jì)期間從中推斷出狀態(tài)。 同樣，此模型因情況而異。 例如，在移動(dòng)機(jī)器人示例中，如果您關(guān)心位置并且擁有GPS，則您的模型就是身份功能，因?yàn)槟呀?jīng)在測(cè)量狀態(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)演化到下一個(gè)時(shí)刻（1）。 這相當(dāng)于說：鑒于我當(dāng)前對(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)行比較。 然后，我們以"智能方式"使用兩者之間的差異來校正狀態(tài)的估計(jì)（3）。

通常，我們用頂點(diǎn)-表示校正前來自預(yù)測(cè)步驟的狀態(tài)估計(jì)。 K稱為卡爾曼增益。 那才是真正的聰明之處：K取決于我們對(duì)測(cè)量的信任程度，我們對(duì)當(dāng)前估計(jì)的信任程度（這取決于我們對(duì)模型的信任程度），并根據(jù)此信息K"決定"了預(yù)測(cè)的估計(jì)量 用測(cè)量值校正。 如果我們的測(cè)量噪聲"小"，而不是我們相信來自預(yù)測(cè)步驟的估計(jì)，那么我們將使用該測(cè)量對(duì)估計(jì)進(jìn)行很多校正，如果相反，那么我們將對(duì)其進(jìn)行最小限度的校正。

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

Example of real position and estimation at each step of the KF algorithm.

肯德基家族

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

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

從歷史上看，人們發(fā)現(xiàn)了兩種主要方法：用模型作弊和用數(shù)據(jù)作弊。如果您對(duì)模型作弊，則基本上可以使當(dāng)前估計(jì)值周圍的非線性函數(shù)線性化，從而使您回到可以工作的線性情況。這種方法稱為擴(kuò)展卡爾曼濾波器（EKF）。該方法的主要缺點(diǎn)是您必須能夠計(jì)算f（）和h（）的雅可比行列式。另外，如果您使用數(shù)據(jù)作弊，則可以使用非線性函數(shù)，但隨后嘗試對(duì)非高斯分布進(jìn)行"高斯化"（如果該詞甚至存在）。這是通過稱為"無味轉(zhuǎn)換"的智能采樣技術(shù)完成的。通過此變換，您可以（均值）和均方差來描述一個(gè)分布（在前兩個(gè)時(shí)刻僅完全描述了高斯分布）。這種方法稱為無味卡爾曼濾波器（UKF）。從理論上講，UKF優(yōu)于EKF，因?yàn)榕c模型線性化相比，Unscented Transform對(duì)結(jié)果分布的近似更好。在實(shí)踐中，必須具有相當(dāng)大的非線性才能實(shí)際看到較大的差異。

肯尼迪行動(dòng)

由于我談到了很多有關(guān)帶GPS的移動(dòng)機(jī)器人的內(nèi)容，因此我就此情況作了簡短的演示（如果要使用它，可以在這里找到代碼）。 使用獨(dú)輪車模型生成機(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í)際位置顯示為黑色），在每一步中，您都會(huì)得到非常嘈雜的GPS測(cè)量值，該測(cè)量值給出x和y（紅色）并估算位置（藍(lán)色）。 您可以使用不同的參數(shù)，看看它們?nèi)绾斡绊憼顟B(tài)估計(jì)。 如您所見，我們可以進(jìn)行非常嘈雜的測(cè)量，并對(duì)實(shí)際位置進(jìn)行很好的估算。

KF in action: a robot real path (black) is tracked with a KF (blue) from noisy measurements (red).

獎(jiǎng)勵(lì)：卡爾曼增益的直觀含義

讓我們看一下線性O(shè)F情況下卡爾曼增益的公式，并嘗試更深入地了解增益的工作原理。

其中P_k是當(dāng)前估計(jì)狀態(tài)的協(xié)方差（我們對(duì)估計(jì)的信心程度），C是測(cè)量模型的線性變換，使得y（k）= Cx（k），R是測(cè)量噪聲的協(xié)方差矩陣 。 請(qǐng)注意，分?jǐn)?shù)表示法并不是真正正確的，但是可以使發(fā)生的事情更容易可視化。

根據(jù)等式，如果R變?yōu)?，則我們有：

代之以定義的算法步驟（3），可以看到我們將完全忽略預(yù)測(cè)步驟的結(jié)果，并且使用測(cè)量模型的逆變換來獲得僅來自測(cè)量的狀態(tài)估計(jì)。

相反，如果我們非常信任模型/估計(jì)，則P_k將趨于0，從而得出：

因此，我們得到的最終估計(jì)與預(yù)測(cè)步長輸出相同。

請(qǐng)注意，我正在交替使用"信任模型"和"信任當(dāng)前估計(jì)"。 它們并不相同，但它們是相關(guān)的，因?yàn)槲覀儗?duì)預(yù)測(cè)步驟的估計(jì)的信任程度是我們對(duì)模型的信任程度（因?yàn)閮H使用模型完成預(yù)測(cè)步驟）加上我們對(duì)模型的信任程度的組合。 估計(jì)上一步過濾。

獎(jiǎng)勵(lì)2：庫

有很多不錯(cuò)的庫可以在線計(jì)算KF，這是我的一些最愛。

作為GO愛好者，我將從這個(gè)非常不錯(cuò)的GO庫開始，其中包含幾個(gè)預(yù)先實(shí)現(xiàn)的模型：rosshemsley/kalman

對(duì)于Python，您可以查看 pykalman.github.io

結(jié)論：我們深入研究了什么是狀態(tài)估計(jì)，卡爾曼濾波器的工作原理，其背后的直覺，如何使用它們以及何時(shí)使用。 我們介紹了一個(gè)玩具（但現(xiàn)實(shí)生活中）的問題，并介紹了如何使用卡爾曼濾波器解決該問題。 然后，我們更深入地研究了Kalman濾波器在幕后的實(shí)際作用。