通俗理解卡爾曼濾波（無人駕駛感知融合的經(jīng)典算法）

netouch 2025-02-01

展開全文

前言

我個(gè)人有近10年AI教育經(jīng)驗(yàn)了,，中間獲得過一些名號,，比如北理工校外導(dǎo)師，微軟MVP兼CSDN技術(shù)專家,，本博客也有1700多萬PV了,，在AI圈內(nèi)有極高知名度。后2015年和團(tuán)隊(duì)一塊創(chuàng)業(yè)創(chuàng)辦AI職教平臺「七月在線」,，至今已近7年,，這6 7年我們把AI職教做透了，同行沒做的我們做,，同行沒有的廣度我們有,，同行不到的深度我們到(本處同行僅指AI社會培訓(xùn)機(jī)構(gòu))。

六年多來,，在專注成人AI職教的同時(shí),，面對過很多行業(yè)的刺激，但自始至終都堅(jiān)持專注AI,?？蛇^程中也越發(fā)深刻意識到，AI只有與行業(yè)深度結(jié)合綁定才能真正發(fā)揮出超級價(jià)值,，考慮到我們的使命始終是“推動(dòng)社會智能化發(fā)展”,，加之國家的制造強(qiáng)國戰(zhàn)略特別提到新能源汽車、智能汽車,，而AI可以很好的賦能自動(dòng)駕駛,，所以一個(gè)月前我司平臺的定位由“AI職教平臺”改為“AI與汽車職教平臺”（后最終為：智能時(shí)代在線職教與行業(yè)解決方案商）。

一個(gè)月的時(shí)間,，我們組織了50位汽車專家/講師（年底100人）,，與原有的300+AI專家/講師區(qū)分開，大家討論中都深感傳統(tǒng)車企向智能化轉(zhuǎn)型的急迫,。偶然中,，在一車企客戶現(xiàn)場聊到感知融合的經(jīng)典算法：卡爾曼濾波（Kalman Filter），故新一篇博客就寫這個(gè),。這算法神到什么程度呢,，比如有人號稱當(dāng)年阿波羅就靠它登陸上的月球 :) ????

關(guān)于卡爾曼濾波,，網(wǎng)上有一篇經(jīng)典文章《How a Kalman filter works, in pictures》，也有不少朋友對它進(jìn)行過翻譯,。我看過幾遍,，覺得還可以把卡爾曼濾波解釋的更通俗些，畢竟把相對復(fù)雜的算法盡可能解釋的更通俗易懂,，從而讓更多初學(xué)者更好更快的理解,，是我現(xiàn)在繼續(xù)更新博客的唯一動(dòng)力了。

此外,，為照顧一些傳統(tǒng)汽車人甚至之前沒聽說過智能網(wǎng)聯(lián),、自動(dòng)駕駛的朋友也能從本文有所收獲，故我會從智能網(wǎng)聯(lián),、無人駕駛開始講起,，直到講透卡爾曼濾波算法，最后到卡爾曼濾波的相關(guān)擴(kuò)展算法,，且過程中得到了盛,、曉、葉等相關(guān)七月講師的校正,，也歡迎讀者批評指正,，thanks。

第一部分智能網(wǎng)聯(lián)與無人駕駛

1.1 什么是自動(dòng)駕駛/無人駕駛

一般而言,，智能網(wǎng)聯(lián)包含了自動(dòng)駕駛和車聯(lián)網(wǎng),，高級別的自動(dòng)駕駛又稱無人駕駛。

現(xiàn)在是個(gè)車企都在往自動(dòng)駕駛方面投入（動(dòng)輒就是上百億）,，到了好像沒有自動(dòng)駕駛,，就不是一輛可以上得了臺面的車，畢竟智能三電（電池,、電機(jī),、電控）、智能座艙,、自動(dòng)駕駛已代表未來汽車的發(fā)展趨勢,，所謂沒自動(dòng)駕駛沒未來。而自動(dòng)駕駛的門檻相對高,，技術(shù)種類也不少,，所以市面上類似的書、課程都不多（比如有一本《第一本無人駕駛技術(shù)書》,，七月在線也曾于2019年推出過一個(gè)《無人駕駛實(shí)戰(zhàn)》,，后面則陸續(xù)推出過目標(biāo)檢測,、目標(biāo)跟蹤,、slam,、感知融合等等小課）。

而無人駕駛的軟件系統(tǒng)一般會被定義為六大模塊（當(dāng)然,，有的模塊會在不同的場景中合并）

高精度地圖（即high dimensional map,，此處的高精度地圖不同于咱們?nèi)粘Ｉ钪械哪欠N導(dǎo)航地圖，而是有著更高維度的信息）,，地圖是感知,、定位、預(yù)測,、規(guī)劃的基礎(chǔ)
定位,，包括且不限于衛(wèi)星導(dǎo)航（比如美國GPS、俄羅斯“格洛納斯”,、歐洲“伽利略”,、中國“北斗”）、慣導(dǎo)（IMU）,、輪速計(jì),、視覺和激光雷達(dá)定位（比如激光雷達(dá)會根據(jù)激光照射到墻壁后反射回來的時(shí)間，來判斷自己和墻壁之間的距離,，從而判斷出自己所處的位置,、實(shí)現(xiàn)定位）
感知，估計(jì)周圍物體的狀態(tài)（比如位置,、速度,、朝向等），一般分為兩個(gè)階段：目標(biāo)檢測（常用手段包括各種深度學(xué)習(xí)方法,，比如圖像2d,、圖像3d、激光雷達(dá)3d,、毫米波點(diǎn)云）,，和目標(biāo)跟蹤（常用手段包括貝葉斯濾波/卡爾曼濾波，以及深度學(xué)習(xí)相關(guān)方法,，例如行人重識別person reid）,，且后者跟蹤基于前者的檢測結(jié)果
行為預(yù)測，預(yù)測周圍行人和車輛的未來軌跡（位置,、速度）
決策規(guī)劃,，包括如何自主避障、如何達(dá)到目的地的路徑規(guī)劃（比如A*,，Dijkstra等算法）
控制,，通過控制執(zhí)行機(jī)構(gòu)（油門、剎車,、方向盤）來完成規(guī)劃的加速,、減速,、轉(zhuǎn)彎等操作

在無人駕駛里有存在四大狀態(tài)的估計(jì)問題：1) 定位，估計(jì)自身狀態(tài),；2）跟蹤,，估計(jì)周圍物體狀態(tài)（比如隨時(shí)間變化的位置速度等）；3）建圖,，估計(jì)周圍靜態(tài)環(huán)境狀態(tài),；4）標(biāo)定，估計(jì)傳感器之間的時(shí)空關(guān)系,。而下文將提到的SLAM（同時(shí)定位與構(gòu)圖技術(shù)）便是解決同時(shí)定位與建圖的問題,。

更具體的，之前七月在線《無人駕駛實(shí)戰(zhàn)》的授課老師盛老師有寫過一篇關(guān)于自動(dòng)駕駛L4級技術(shù)的短文,，現(xiàn)引用部分內(nèi)容如下：

無人駕駛要解決4 個(gè)關(guān)鍵問題：我在哪,？我周圍有什么？接下來會發(fā)生什么,？我應(yīng)該怎么做,？

我在哪？定位,、高精地圖

定位需要依靠一種稱為高精地圖的技術(shù),，該技術(shù)會將無人車要走的所有靜態(tài)環(huán)境進(jìn)行描述，包括車道線,、行人斑馬線,、標(biāo)志牌等等。這些靜態(tài)信息可以提供交通信號的關(guān)鍵信息,，也會作為定位方案的錨定物對自身的位置進(jìn)行校準(zhǔn),，比如通過攝像頭看到距離左邊標(biāo)志牌的距離是2.5m，那么在地圖中知道了標(biāo)志牌的坐標(biāo)也就知道了自身車輛的坐標(biāo),。同時(shí),，還會依靠GPS/IMU等全局設(shè)備來定位自身位置，不過這可比我們目前智能手機(jī)里的GPS精度要求高很多,，通過差分融合技術(shù)可以達(dá)到厘米級精度,。

我周圍有什么？視覺感知,、激光雷達(dá)等
有了定位后,，無人車的感知系統(tǒng)將通過傳感器和AI算法將周圍的障礙物位置、大小,、狀態(tài),、類別等標(biāo)識出來。目前主流L4級別的傳感器包括GPS/IMU、LIDAR,、Camera,、Radar等，后三者都是用于感知周圍障礙物的主要傳感器,，分別在不同環(huán)境下能夠有不同的優(yōu)勢,。這些信息猶如人類駕駛員的眼睛一樣看到周圍動(dòng)態(tài)環(huán)境物體,，并將其識別出來,，比如精準(zhǔn)識別車輛后方任何物體、同時(shí)關(guān)注左右兩邊的車輛狀態(tài),，在黑暗狀態(tài)時(shí)可以通過激光雷達(dá)精準(zhǔn)識別,。

接下來會發(fā)生什么？行為預(yù)測,、速度預(yù)測
無人車知道周圍動(dòng)態(tài)物體后,，還需要能夠盡可能的預(yù)測這些物體的走向，包括行為預(yù)測和速度預(yù)測,。例如這輛車是要左轉(zhuǎn)還是直行,，那輛車會不會闖紅燈等等，匯入車流時(shí)速度是多少,。這些問題都將決定我們無人車后續(xù)應(yīng)該怎么走,，如何避免碰撞發(fā)生危險(xiǎn)。

我應(yīng)該怎么做,？決策,、規(guī)劃、控制
最后一步就是根據(jù)上述信息綜合來選擇一條最適合無人車的道路,，如同人類的大腦一樣對車輛最終的行為負(fù)責(zé),，選擇最合適的方式達(dá)到目的地。這需要考慮行車的體感,、安全和快捷等因素,，通過最優(yōu)化算法、搜索算法,、蒙特卡洛樹采樣等多種算法來得到未來的駕駛行為,。

1.2 智能網(wǎng)聯(lián)相關(guān)崗位

針對上文智能網(wǎng)聯(lián)汽車的定義，可以看出涉及的技術(shù)不少,，每項(xiàng)技術(shù)背后都對應(yīng)著一個(gè)個(gè)崗位

我們來具體分析其中幾個(gè)崗位,，比如視覺SLAM算法工程師。

那什么是slam呢,？我們知道,，傳統(tǒng)的圖像處理基本上是基于二維圖像，而人眼是通過兩只眼睛的視差（雙目立體視覺）來感知周圍的三維空間。隨著三維視覺傳感器的飛速發(fā)展,，三維視覺核心技術(shù)：SLAM（全稱 Simultaneous Localization And Mapping,，譯為同時(shí)定位與地圖構(gòu)建）技術(shù)，本質(zhì)是對運(yùn)動(dòng)主體自身和周遭環(huán)境不確定性的估計(jì),，相當(dāng)于核心解決兩個(gè)問題：1 因?yàn)榈貓D是未知的,，所以需要估計(jì)地圖（周遭環(huán)境），2 位置也是未知的,，所以需要估計(jì)位置,。至于常用手段則有攝像頭，也有通過各種激光雷達(dá)探測距離或獲取周遭環(huán)境圖像的,。

視覺SLAM中的傳感器即為攝像頭（通過攝像頭同時(shí)解決定位與地圖的問題）

攝像頭如果使用深度相機(jī),，可以直接獲取障礙物的距離，生成點(diǎn)云數(shù)據(jù)
當(dāng)然也有用單目,、雙目,、魚眼攝像機(jī)的視覺SLAM ，這些攝像頭需要用其他的方法才能間接的獲取周圍障礙物的距離

可能有同學(xué)問了,，如果從零學(xué)視覺SLAM都要求掌握哪些技術(shù)呢（當(dāng)然,，以下所有這些技術(shù)都可以通過這個(gè)slam課掌握好）？

常用的操作系統(tǒng)也是Linux,，這和我們做IT后臺開發(fā)或AI算法研發(fā)一樣,，不懂Linux寸步難行，學(xué)校沒學(xué)的,，都得在找工作中或公司里抽時(shí)間惡補(bǔ),；
編程語言：C、C++,，在如今AI特別火的時(shí)代,，Python可以說是數(shù)據(jù)科學(xué)領(lǐng)域第一語言了，通過Python入門AI確實(shí)是不錯(cuò)的選擇,，但入門之后,，真想在IT、AI,、汽車行業(yè)真正深入發(fā)展,，只懂一門Python可能就不夠了，第二門語言可以是C/C++（C++ primer,、深度理解C++對象模型等經(jīng)典書都不錯(cuò)）,，也可以是Java（這里也有個(gè)Java與數(shù)據(jù)結(jié)構(gòu)課）
SLAM相關(guān)的數(shù)學(xué)知識，包括且不限于：線性代數(shù)基礎(chǔ)（矩陣操作）,、三維空間的剛體運(yùn)動(dòng),、SLAM中空間點(diǎn)與不同的坐標(biāo)系,、旋轉(zhuǎn)的表達(dá)（四元數(shù)、旋轉(zhuǎn)矩陣,、旋轉(zhuǎn)向量,、歐拉角）、李群與李代數(shù),、相機(jī)成像及常用視覺傳感器,、最小二乘法
SLAM核心技術(shù)點(diǎn)：視覺里程計(jì)、多視角幾何,、視覺里程計(jì)中的位姿估計(jì)方法 ,、非線性優(yōu)化（比如卡爾曼濾波）、回環(huán)檢測與重建常用的vSLAM算法,，如ORB-SLAM,、SVO,、DSO,、MonoSLAM，VINS以及RGB-D相機(jī)等
卡爾曼濾波算法的相關(guān)算法,，比如擴(kuò)展卡爾曼濾波,、無跡卡爾曼濾波、誤差狀態(tài)卡爾曼濾波器,，英文簡寫分別為EKF,、UKF、ESKF等
ROS機(jī)器人操作系統(tǒng),；

對于以上六點(diǎn),，有兩點(diǎn)值得再提的是：比如針對第3點(diǎn)，不要忌憚數(shù)學(xué),，因?yàn)?u>凡是涉及到數(shù)據(jù)相關(guān)的技術(shù),，最后都是算法，最深入都是數(shù)學(xué)（可能你此刻還不這么覺得,，但當(dāng)你看完全文后你一定會深以為然）,；針對第4點(diǎn)，下面是“RGB-D相機(jī)中的直接法運(yùn)用”的示意圖

再比如視覺感知算法工程師,，會涉及

感知算法研發(fā)和系統(tǒng)實(shí)現(xiàn),，基于視覺的障礙物檢測、跟蹤,、行為預(yù)測等,；
感知的視覺幾何計(jì)算三維重建，目標(biāo)檢測跟蹤,、圖像分割,、圖像識別等算法研發(fā),；

最后，再比如傳感器融合算法高級工程師,，這個(gè)崗位的崗位職責(zé)一般是：通過融合攝像頭,、激光雷達(dá)、毫米波雷達(dá)等傳感器的感知信息,，實(shí)現(xiàn)對周圍動(dòng)靜環(huán)境的精確描述及預(yù)測,，具體包括

多傳感器多目標(biāo)跟蹤與意圖預(yù)測,，涉及感知信息的預(yù)處理,、目標(biāo)創(chuàng)建、關(guān)聯(lián),、狀態(tài)估計(jì),、合并與刪除等等,；
構(gòu)建高性能的3D感知融合系統(tǒng);
設(shè)計(jì)開發(fā)基于毫米波雷達(dá)稀疏點(diǎn)云的目標(biāo)檢測;
柵格地圖\靜態(tài)環(huán)境實(shí)時(shí)構(gòu)建。

反應(yīng)到崗位要求,，一般有以下幾點(diǎn)

扎實(shí)的編程能力,，熟悉在Linux環(huán)境下進(jìn)行C++\Python開發(fā)，理解常用數(shù)據(jù)結(jié)構(gòu)及STL使用,；
深入理解卡爾曼濾波等貝葉斯濾波算法,，能夠熟練地進(jìn)行數(shù)學(xué)建模及其狀態(tài)估計(jì)，深入理解常用的數(shù)據(jù)關(guān)聯(lián)算法與距離計(jì)算方法,；
熟悉攝像頭,、激光雷達(dá)、毫米波雷達(dá)等傳感器,，熟悉計(jì)算機(jī)視覺及其應(yīng)用,，有SLAM\Deep Learning基礎(chǔ)更佳；

可見,，卡爾曼濾波在感知融合里應(yīng)用很廣,，可以對各種傳感器獲取到的數(shù)據(jù)進(jìn)行進(jìn)一步的綜合處理，達(dá)到更佳的估計(jì)狀態(tài),。

第二部分卡爾曼濾波算法

2.1 什么是卡爾曼濾波

關(guān)于什么是卡爾曼濾波,，下面是維基百科上的解釋（當(dāng)然，為了通俗起見,，我修改了部分描述）

?卡爾曼濾波（Kalman filter）是一種高效率的遞歸濾波器（自回歸濾波器）,，它能夠從一系列的不完全及包含噪聲的測量中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài),?？柭鼮V波會根據(jù)各測量量在不同時(shí)間下的值，考慮各時(shí)間下的聯(lián)合分布,，再產(chǎn)生對未知變數(shù)的估計(jì),，因此會比只以單一測量量為基礎(chǔ)的估計(jì)方式要準(zhǔn),。卡爾曼濾波得名自主要貢獻(xiàn)者之一的魯?shù)婪颉た柭?/p>
卡爾曼濾波的算法程序有兩個(gè)步驟,，分別是估計(jì),、更新：

在估計(jì)步驟中，卡爾曼濾波會產(chǎn)生有關(guān)目前狀態(tài)的估計(jì),，其中也包括不確定性,；
在更新步驟中，只要觀察到下一個(gè)量測（其中一定含有某種程度的誤差,，包括隨機(jī)噪聲）,，會通過加權(quán)平均來修正估計(jì)值，而確定性越高的量測加權(quán)比重也越高,。

算法是迭代的,，可以在實(shí)時(shí)控制系統(tǒng)中執(zhí)行，只需要目前的輸入量測,、以往的計(jì)算值以及其不確定性矩陣,，不需要其他以往的資訊。

卡爾曼濾波之后,，后續(xù)發(fā)展了一些擴(kuò)展或是廣義的卡爾曼濾波,，例如運(yùn)作在非線性系統(tǒng)的擴(kuò)展卡爾曼濾波及無跡卡爾曼濾波（Unscented Kalman filter）,。底層的模型類似隱馬爾可夫模型,，不過潛在變量的狀態(tài)空間是連續(xù)的，而且所有潛在變量及可觀測變數(shù)都是正態(tài)分布,。

恩,，Wikipedia上的定義向來準(zhǔn)確嚴(yán)謹(jǐn)，唯一問題就是不夠通俗,、不夠易懂,，怎么理解更好懂呢？很簡單,，抽絲剝繭,，一個(gè)算法相對復(fù)雜，一般來講是因?yàn)樯婕暗谋尘爸R比較多,，那咱們就來一一剖析,，切開了揉碎了自然就好懂了。

首先,，回顧一下正太分布（又稱高斯分布,，一個(gè)意思）的定義

至于數(shù)學(xué)期望和方差則分別如下圖所示

其次，通過一個(gè)例子理解下卡爾曼濾波的思想,。

1 假設(shè)你現(xiàn)在有一輛車,，并且已知小車在初始狀態(tài)的位置如下圖所示（小車的位置服從正態(tài)分布,，比如可能位于正態(tài)分布的均值附近），橫坐標(biāo)為小車前進(jìn)坐標(biāo),。

有人會問,，為什么是正態(tài)分布而不是確定點(diǎn)呢？因?yàn)樵谶@里我們給出的是它此刻的狀態(tài)估計(jì)量,，對于地圖上的物體,，我們無法確切得出其具體的位置信息，即便你使用GPS來定位也始終會有一定的誤差,。因此你只能通過系統(tǒng)的輸入（如小車的速度）和系統(tǒng)的輸出（GPS給定的位置信息）來估算它當(dāng)前的狀態(tài)（真實(shí)的位置信息）,。

在這里插入圖片描述

2 然后，根據(jù)初始時(shí)刻小車的位置,，我們可以預(yù)測出它在下一個(gè)時(shí)間步長的可能的位置分布,。下圖中可以看出，它的正態(tài)分布函數(shù)明顯比上一時(shí)刻來的更寬一些,。這是因?yàn)樵谛≤囘\(yùn)動(dòng)過程中會不可避免地遇到一些隨機(jī)因素的干擾,，如坑洼、風(fēng),、輪子打滑等,，因此我們估算的位置值的不確定性變大，小車實(shí)際經(jīng)過的距離與模型預(yù)測的距離不同,。

在這里插入圖片描述

3 到此刻為止,，上述過程均是由我們的數(shù)學(xué)模型推算估計(jì)出來的結(jié)果，但實(shí)際上,，相同的數(shù)據(jù)也可以通過傳感器測量來獲取,，而測量值往往也呈正態(tài)分布（總之，頻率派里未知值是一個(gè)確定的值,，在貝葉斯派里,，未知值都是一個(gè)概率分布，此點(diǎn)不用皺眉,，很快你會有更深刻的認(rèn)知）,，并且與我們的模型預(yù)測值有所不同，如下圖中紅色正態(tài)分布曲線所示,，方差表示噪聲測量中的不確定性,。

在這里插入圖片描述

4 這個(gè)時(shí)候咋辦呢？可能你會脫口而出：將數(shù)學(xué)模型的預(yù)測值與傳感器的測量值相互結(jié)合起來來獲得小車的最佳位置估計(jì)值,，怎么結(jié)合呢,？加權(quán)平均再迭代！具體到這個(gè)例子中,，咱們把這兩者的概率密度函數(shù)相乘得出了當(dāng)前時(shí)刻的最佳狀態(tài)估計(jì)值,，如圖中灰色正態(tài)分布曲線所示,，這就是卡爾曼濾波的核心思想，因此卡爾曼濾波也經(jīng)常被稱作傳感器融合算法,。

在這里插入圖片描述

再然后,，我們把前面的時(shí)間信息換一下，比如我們知道了上一時(shí)刻即k-1時(shí)刻小車的位置估算值,，并且通過數(shù)學(xué)模型預(yù)測得到了k時(shí)刻的位置估算值,，然后將其與小車傳感器的測量值進(jìn)行結(jié)合，得到了當(dāng)前k時(shí)刻小車的最佳位置估算值,。

那么問題來了,，這里我們并沒有給出初始狀態(tài)，因此k-1時(shí)刻的位置估算值又是怎么來的呢,？沒錯(cuò),，我們是通過k-2時(shí)刻的狀態(tài)量估算得來了。

2.2 卡爾曼濾波的完整流程與數(shù)學(xué)推導(dǎo)

通過上一節(jié)的例子,，我們知曉了卡爾曼濾波的核心思想,，接下來我們通過另一個(gè)例子來討論其完整流程與數(shù)學(xué)建模推導(dǎo)下的細(xì)節(jié)、原理,。

2.2.1 針對機(jī)器人的位置和速度建模

假如你開發(fā)了一款小型機(jī)器人,，它可以在樹林里自主移動(dòng)。

我們可以通過一組狀態(tài)變量 $\bar{x}$ 來描述機(jī)器人的狀態(tài),，包括位置和速度：

$\overrightarrow{x_{k}}=(\vec{p}, \vec{v})$

下面,，我們需要機(jī)器人需要明確自己的位置以便進(jìn)行導(dǎo)航。怎么確定位置呢,？

首先,，我們給機(jī)器人安了一個(gè)GPS傳感器，但精度只在10m,。考慮到對于在機(jī)器人所處的樹林里有很多溪谷和斷崖,，如果機(jī)器人對位置誤判了哪怕只是幾步遠(yuǎn)的距離,，它都有可能掉到坑里，所以僅靠這個(gè)精度的GPS來定位是不夠的,。

我們得有更多的辦法確定位置,。比如我們可以獲取到一些機(jī)器人的運(yùn)動(dòng)的信息：驅(qū)動(dòng)輪子的電機(jī)指令。如果沒有外界干擾,，僅僅是朝一個(gè)方向前進(jìn),，那么下一個(gè)時(shí)刻的位置只是比上一個(gè)時(shí)刻的位置在該方向上移動(dòng)了一個(gè)固定距離。但如果有外界干擾呢,？比如逆風(fēng)行駛,，輪子打滑,，滾落顛簸地形……所以輪子轉(zhuǎn)過的距離并不能完全表示機(jī)器人移動(dòng)的距離，這就導(dǎo)致通過輪子轉(zhuǎn)動(dòng)預(yù)測機(jī)器人的位置不會非常準(zhǔn)確,。

如此,，相當(dāng)于

GPS傳感器會告知我們一些關(guān)于機(jī)器人狀態(tài)的信息，但是GPS可能不準(zhǔn),；
另外,，通過輪子轉(zhuǎn)動(dòng)可以預(yù)知機(jī)器人運(yùn)動(dòng)的距離，可同樣也有一定的不準(zhǔn)確度（受外力干擾）,。

行到水窮處換個(gè)思路：如果我們綜合“GPS + 輪子轉(zhuǎn)動(dòng)預(yù)測”兩者的信息呢,？是否可以得到比只依靠單獨(dú)一個(gè)信息來源更精確的結(jié)果？顯然,，這就是卡爾曼濾波要解決的問題,。

強(qiáng)調(diào)一下，前言中所說的這篇文章《How a Kalman filter works, in pictures》,，形象的闡述了卡爾曼濾波的算法流程,，我看過幾遍，覺得還可以把卡爾曼濾波解釋的更通俗些,，所以我在原文圖片和公式的基礎(chǔ)上加了很多原文沒有且網(wǎng)上其他文章也沒有的大量解釋,、說明，以提高文章的可讀性,。

針對上邊的系統(tǒng),，我們建立包括位置和速度的系統(tǒng)狀態(tài)：

我們不知道位置和速度的準(zhǔn)確值，但是我們可以列出一個(gè)準(zhǔn)確數(shù)值可能落在的區(qū)間范圍,。在這個(gè)區(qū)間范圍里,，一些數(shù)值組合的可能性要高于另一些組合的可能性。

卡爾曼濾波假設(shè)所有的變量（在我們的例子中為位置和速度）是隨機(jī)的且符合正態(tài)分布,。每個(gè)變量有一個(gè)均值ｕ代表了隨機(jī)分布的中心值（代表這是可能性最大的值）,，和一個(gè)方差 $\sigma^{2}$ ，用于衡量不確定度,。

在上圖中,，位置和速度是不相關(guān)的，這意味著我們不能從一個(gè)變量推測另一個(gè)變量,。

實(shí)際上呢,，位置和速度是相關(guān)的。比如已知上一個(gè)狀態(tài)的位置值,，現(xiàn)在要預(yù)測下一個(gè)狀態(tài)的位置值：

如果機(jī)器人的速度很快,，一定的時(shí)間內(nèi)，機(jī)器人可能移動(dòng)得很遠(yuǎn)；
相反,，如果速度很慢,，同樣的時(shí)間內(nèi)，機(jī)器人會移動(dòng)的很近,。

這種關(guān)系在跟蹤系統(tǒng)狀態(tài)時(shí)很重要,，因?yàn)樗o了我們更多的信息：一個(gè)測量值告訴我們另一個(gè)測量值可能是什么樣子。

為了捕獲這種相關(guān)性,，我們可以使用協(xié)方差矩陣,。簡而言之，矩陣中的每個(gè)元素 $\Sigma_{i j}$ 表示了第個(gè)變量和第個(gè)變量之間的關(guān)系（協(xié)方差矩陣是對稱的,，即交換下標(biāo)和并無任何影響）,。

協(xié)方差矩陣通常表示為Σ，它的元素則表示為 $\Sigma_{i j}$ ,，從而有

$\Sigma_{i j}=\operatorname{Cov}\left(x_{i}, x_{j}\right)=E\left[\left(x_{i}-\mu_{i}\right)\left(x_{j}-\mu_{j}\right)\right]$

2.2.2 利用矩陣進(jìn)一步描述系統(tǒng)狀態(tài)

為了把以上關(guān)于狀態(tài)的信息建模為正態(tài)分布,，所以在k時(shí)刻我們需要兩個(gè)信息：最佳預(yù)估值 $\bar{x}_{k}$ （均值，也就是u）,，和它的協(xié)方差矩陣 $P_{k}$

(這里我們只記錄了位置和速度,，但只要和問題相關(guān)，我們可以把任何數(shù)據(jù)變量放進(jìn)系統(tǒng)狀態(tài)里)

接下來,，我們要通過k-1時(shí)的狀態(tài)來預(yù)測下一個(gè)狀態(tài)（k時(shí)）,。但我們不知道狀態(tài)的準(zhǔn)確值，怎么辦呢,？預(yù)測函數(shù)會對k-1時(shí)刻所有可能值的范圍進(jìn)行預(yù)測,，然后得出一個(gè)k時(shí)刻新值的分布范圍。

我們可以通過一個(gè)狀態(tài)轉(zhuǎn)移矩陣 $F_{k}$ 來表示這個(gè)預(yù)測過程,， $F_{k}$ 把k-1時(shí)刻所有可能的狀態(tài)值轉(zhuǎn)移到一個(gè)新的范圍內(nèi)（它從原始預(yù)測中取每一點(diǎn),，并將其移動(dòng)到新的預(yù)測位置），這個(gè)新的范圍代表了系統(tǒng)新的狀態(tài)值可能存在的范圍,，如果k-1時(shí)刻估計(jì)值的范圍是準(zhǔn)確的話,。

考慮到勻速狀態(tài)下，S（路程）=v（速度）x t（時(shí)間）,，我們通過下述公式來描述這個(gè)'預(yù)測下個(gè)狀態(tài)'的過程（別忘了,， $\overrightarrow{x_{k}}=(\vec{p}, \vec{v})$ ）：

進(jìn)而整理為矩陣則是：

我們現(xiàn)在有了一個(gè)預(yù)測矩陣，可以簡單預(yù)測下個(gè)狀態(tài),，但怎么更新協(xié)方差矩陣呢？

試想,，如果我們對每個(gè)點(diǎn)進(jìn)行矩陣A轉(zhuǎn)換,，它的協(xié)方差矩陣Σ會發(fā)生什么變化，比如

結(jié)合（4）和（3）：

2.2.3 不確定外部因素的估計(jì)：貝葉斯理論

到目前為止,，我們還沒有考慮到所有的影響因素,，因?yàn)橄到y(tǒng)狀態(tài)的改變并不只依賴上一個(gè)系統(tǒng)狀態(tài),，外界作用力也可能會影響系統(tǒng)狀態(tài)的變化。外界作用力分兩種

第一種是人為指令,，例如,，在我們跟蹤一列火車的運(yùn)動(dòng)狀態(tài)時(shí)，火車駕駛員可能踩了油門使火車提速,。
第二種是不受人為控制,，比如2.1節(jié)提到的坑洼、風(fēng),、輪子打滑等等,，其所帶來的變化是未知且不好預(yù)測的，沒法嚴(yán)格確定到某個(gè)具體的值,。恩,，有沒有熟悉的貝葉斯理論的味道了？

對于貝葉斯的理論有必要提一下（特引用我之前寫的一篇貝葉斯筆記,，詳見參考文獻(xiàn)5）

先看兩個(gè)例子

比如有一個(gè)袋子,，里面裝著若干個(gè)白球和黑球，請問從袋子中取得白球的概率θ是多少,？”

頻率派的人會想都不用想而立馬告訴你,，取出白球的概率 $\theta$ 就是1/2，要么取到白球,，要么取不到白球,，即θ只能有一個(gè)值，而且不論你取了多少次,，取得白球的概率θ始終都是1/2,，即不隨觀察結(jié)果X 的變化而變化。

貝葉斯的理論則認(rèn)為取得白球的概率是個(gè)不確定的值,，因?yàn)槠渲泻袡C(jī)遇的成分,。

再比如，一個(gè)朋友創(chuàng)業(yè),，你明明知道創(chuàng)業(yè)的結(jié)果就兩種,，即要么成功要么失敗，但你依然會忍不住去估計(jì)他創(chuàng)業(yè)成功的幾率有多大,？你如果對他為人比較了解,，而且有方法、思路清晰,、有毅力,、且能團(tuán)結(jié)周圍的人，你會不由自主的估計(jì)他創(chuàng)業(yè)成功的幾率可能在80%~90%這個(gè)區(qū)間范圍。這種不同于最開始的“非黑即白,、非0即1”的思考方式,，便是貝葉斯式的思考方式。

換言之

頻率派把需要推斷的參數(shù)θ看做是固定的未知常數(shù),，即概率雖然是未知的,，但最起碼是確定的一個(gè)值，同時(shí),，樣本X 是隨機(jī)的,，所以頻率派重點(diǎn)研究樣本空間，大部分的概率計(jì)算都是針對樣本X 的分布,；
而貝葉斯派的觀點(diǎn)則截然相反,，他們認(rèn)為參數(shù)是隨機(jī)變量，而樣本X 是固定的,，由于樣本是固定的,，所以他們重點(diǎn)研究的是參數(shù)的分布。

針對第一種外力,，在我們機(jī)器人例子中,，導(dǎo)航軟件可能發(fā)出一些指令啟動(dòng)或者制動(dòng)輪子。這個(gè)好解決,，通過一個(gè)向量 $\bar{u}_{k}$ 來描述這些信息,，把它添加到我們的預(yù)測方程里作為一個(gè)修正。

假如我們通過發(fā)出的指令得到預(yù)期的加速度a,，根據(jù)速度-加速度公式 $V_{t} = V_{0} + at$ ,，以及加速度-位移公式 $S = v_0t + \frac{1}{2} at^{2}$ ，上邊的運(yùn)動(dòng)方程可以變化為：

再次整理為矩陣則是：

$B_{k}$ 稱作控制矩陣,， $\bar{u}_{k}$ 稱作控制向量（當(dāng)沒有任何外界動(dòng)力影響的系統(tǒng)時(shí),，則可以忽略該項(xiàng)）。

針對第二種外力,，則相對麻煩些了,，比如受到風(fēng)力影響或輪子打滑、地面上的坑坑洼洼,、突起異物都使得我們跟蹤的物體的速度發(fā)生不可控的變化,。

怎么辦呢？可以把“世界”中的這些不確定性統(tǒng)一建模,，在預(yù)測步驟中增加一個(gè)不確定項(xiàng),。

相當(dāng)于原始狀態(tài)中的每一個(gè)點(diǎn)可以都預(yù)測轉(zhuǎn)換到一個(gè)范圍，而不是某個(gè)確定的點(diǎn),。轉(zhuǎn)換為數(shù)學(xué)表達(dá)是： $\bar{x}_{k-1}$ 中的每個(gè)點(diǎn)移動(dòng)到一個(gè)符合協(xié)方差 $Q_{k}$ 的高斯分布里（也就是藍(lán)色的高斯分布移動(dòng)到紫色高斯分布的位置）,，或者說,，把這些不確定的外部因素描述為協(xié)方差為 $Q_{k}$ 的高斯噪聲,，具體根據(jù)預(yù)測的傳感器噪聲來定（比如imu或者輪速計(jì)）：

加入外部因素協(xié)方差后,，擴(kuò)大后的紫色高斯分布和原來沒有外部因素的小范圍紫色高斯分布相比，兩者之間有相同的均值,，但協(xié)方差不同

對 $Q_{k}$ 簡單疊加,，可以拿到擴(kuò)展的協(xié)方差，這樣就得到了預(yù)測步驟的完整表達(dá)式：

解釋下上面（7）中的那兩個(gè)式子

新的最佳估計(jì)是在原最佳估計(jì)的基礎(chǔ)上,，通過已知的外部影響校正得到的預(yù)測值,。
新的不確定性則綜合了原不確定性和不確定的外部因素的影響得到的預(yù)測。

到這里,，我們得到了一個(gè)模糊的估計(jì)范圍,，一個(gè)通過 $x_{k}$ 和 $P_{k}$ 描述的范圍。如果再結(jié)合我們傳感器的數(shù)據(jù)呢,？

2.2.4 數(shù)學(xué)模型之外再通過測量值修正預(yù)測值

實(shí)際生活當(dāng)中,，我們經(jīng)常會用到不止一種手段來多方論證我們的預(yù)測結(jié)果，上節(jié)我們通過建立數(shù)學(xué)模型預(yù)測,，此外,，我們可能還有一些傳感器來測量系統(tǒng)的狀態(tài)。

當(dāng)然,，我們可能有好幾個(gè)傳感器,，比如一個(gè)測量位置一個(gè)測量速度，每個(gè)傳感器可以提供一些關(guān)于系統(tǒng)狀態(tài)的數(shù)據(jù)信息（每個(gè)傳感器檢測一個(gè)系統(tǒng)變量并且產(chǎn)生一些讀數(shù)）,。

考慮到傳感器測量的范圍和單位可能與我們跟蹤系統(tǒng)變量所使用的范圍和單位不一致,，所以需要對傳感器做下建模：通過矩陣 $H_{k}$ 。

初學(xué)者剛看到這的時(shí)候可能會有疑問,，這個(gè) $H_{k}$ 到底是干啥用的,？

通過上文2.1節(jié)，我們已經(jīng)知曉,，卡爾曼濾波實(shí)質(zhì)就是將預(yù)測狀態(tài)量的高斯分布和觀測量的高斯分布做融合,，生成一個(gè)新的高斯分布。

而很快你會通過下文發(fā)現(xiàn),，這個(gè)新的高斯分布的均值和方差是兩個(gè)獨(dú)立的高斯分布的相關(guān)參量的加權(quán),，這個(gè)加權(quán)就是卡爾曼增益，但是預(yù)測狀態(tài)量和觀測量可能維度不同,，需要將他們同時(shí)轉(zhuǎn)換到一個(gè)向量空間中,，所以觀測量有了線性變換矩陣 $H_{k}$ ，相當(dāng)于狀態(tài)域通過 $H_{k}$ 轉(zhuǎn)換到觀測域,。

為避免降低本文的可讀性,，再舉個(gè)例子,，比如車速可以映射到輪速計(jì)的轉(zhuǎn)速，車速到轉(zhuǎn)速的轉(zhuǎn)換關(guān)系就可以通過 $H_{k}$ 表達(dá),。

把得到的傳感器讀數(shù)的分布范圍轉(zhuǎn)換為一般形式：

換言之,，我們的傳感器也是有自己的精度范圍的，對于一個(gè)真實(shí)的位置和速度,，傳感器的讀數(shù)受到高斯噪聲的影響會在某個(gè)范圍內(nèi)波動(dòng),。

我們觀測到的每個(gè)數(shù)據(jù)，可以認(rèn)為其對應(yīng)某個(gè)真實(shí)的狀態(tài),。但是因?yàn)榇嬖诓淮_定性,，某些狀態(tài)的可能性比另外一些的可能性更高。

我們將這種測量值與真實(shí)值之間的不確定性（即觀測傳感器噪聲）的協(xié)方差設(shè)為 $R_{k}$ ,，均值設(shè)為 $\bar{z}_{k}$ ,，具體根據(jù)觀測傳感器的噪聲來定（比如相機(jī)、lidar）,。

所以現(xiàn)在我們有了兩個(gè)數(shù)據(jù),，一個(gè)來自于我們通過數(shù)學(xué)模型的預(yù)測值，另一個(gè)來自傳感器的測量值,。

我們必須嘗試去把兩者的數(shù)據(jù)預(yù)測值（粉色）與測量值（綠色）融合起來,。

實(shí)際上對于任何狀態(tài)，都有兩個(gè)可能性：

傳感器讀數(shù)更接近系統(tǒng)真實(shí)狀態(tài)
預(yù)測值更接近系統(tǒng)真實(shí)狀態(tài)

怎么融合呢,？概率論里,，兩個(gè)事件如果同時(shí)發(fā)展則相乘，所以我們嘗試將兩個(gè)高斯塊（Gaussian blob）相乘試下：

相乘之后得到的即為重疊部分,，這個(gè)區(qū)域同時(shí)屬于兩個(gè)高斯塊,，并且比單獨(dú)任何一個(gè)區(qū)域都要精確。這個(gè)區(qū)域的平均值取決于我們更取信于哪個(gè)數(shù)據(jù)來源,，這樣通過我們手中的數(shù)據(jù)得到了一個(gè)最好的估計(jì)值,。

恩，又是另一個(gè)高斯塊,。

事實(shí)證明,，當(dāng)你把兩個(gè)高斯分布相乘時(shí)（各自的均值和協(xié)方差矩陣相乘），你會得到一個(gè)擁有獨(dú)立均值和協(xié)方差矩陣的新高斯分布,。最后剩下的問題就不難解決了：新高斯分布的均值和方差均可以通過老的均值方差求得,！

2.2.5 融合兩個(gè)高斯斑：卡爾曼增益出場

首先，針對一維數(shù)據(jù)開始,，一維高斯（均值u,，方差 $\sigma^{2}$ ）的方程被定義為：

如果兩個(gè)高斯分布相乘會發(fā)生什么呢（如之前所說，預(yù)測值是粉紅色,，測量值是綠色）,，下圖的藍(lán)色曲線代表了兩個(gè)高斯分布的交集部分：

把（9）帶入（10）然后做一些變換,，可以得到

且慢，上述公式11的推導(dǎo)如果不一步步展開,，則會導(dǎo)致初學(xué)的你在千篇一律的人云亦云中看的還是很頭疼,，所以我們把上面的變換展開下：

$N(x,u_{0},\sigma_{0}) \cdot N(x,u_{1},\sigma_{1}) \\ = \frac{1}{\sigma_{0} \sqrt{2 \pi}} e^{-\frac{(x-\mu_{0})^{2}}{2 \sigma_{0}^{2}}} \cdot \frac{1}{\sigma_{1} \sqrt{2 \pi}} e^{-\frac{(x-\mu_{1})^{2}}{2 \sigma_{1}^{2}}} \\ = \frac{1}{\sqrt{2 \pi}}\frac{1}{\sigma_{0}\sigma_{1} \sqrt{2 \pi}} e -\frac{\sigma_{1}^{2}\left(x-u_{0}\right)^{2}+\sigma_{0}^{2}\left(x-u_{1}\right)^{2}}{2\left(\sigma_{0} \sigma_{1}\right)^{2}} \\ = \frac{1}{\sqrt{2 \pi}}\frac{1}{\sigma_{0}\sigma_{1} \sqrt{2 \pi}} e- \frac{( \sigma_{0}^{2} + \sigma_{1}^{2})x^2 - 2\left(u_{0}\sigma_{1}^{2}+u_{1} \sigma_{0}^2\right)x + \sigma_{1}^2u_{0}^2 + \sigma_{0}^2u_{1}^2 } { 2(\sigma_{0}\sigma_{1})^2} \\ = \frac{1}{\sqrt{2 \pi}}\frac{1}{\sigma_{0}\sigma_{1} \sqrt{2 \pi}} e- \frac{x^{2}-2\left(\frac{u_{0} \sigma_{1}^{2}+u_{1} \sigma_{0}^{2}}{\sigma_{0}^{2}+\sigma_{1}^{2}}\right) x+\frac{\sigma_{1}^{2} u_{0}^{2}+\sigma_{0}^{2} u_{1}^{2}}{\sigma_{0}^{2}+\sigma_{1}^{2}}}{2 \frac{\left(\sigma_{0} \sigma_{1}\right)^{2}}{\sigma_{0}^{2}+\sigma_{1}^{2}}}$

$=A \cdot \frac{1}{\frac{\sigma_{0} \sigma_{1}} {\sqrt{\sigma_{0}^{2}+\sigma_{1}^{2}}} \sqrt{2 \pi}} e^ {- \frac {\left(x-\frac{u_{0} \sigma_{1}^{2}+u_{1} \sigma_{0}^{2}}{\sigma_{0}^{2}+\sigma_{1}^{2}}\right)^{2}} {2\left (\frac{\sigma_{0} \sigma_{1}} {\sqrt{\sigma_{0}^{2}+\sigma_{1}^2}}\right)^{2}}}$

其中，A代表其它項(xiàng),，從而有

$u^{\prime}=\frac{u_{0} \sigma_{1}^{2}+u_{1} \sigma_{0}^{2}}{\sigma_{0}^{2}+\sigma_{1}^{2}}=u_{0}+\frac{\left(u_{1}-u_{0}\right) \sigma_{0}^{2}}{\sigma_{0}^{2}+\sigma_{1}^{2}}$

$\sigma^{\prime}=\frac{\sigma_{0}^{2} \sigma_{1}^{2}}{\sigma_{0}^{2}+\sigma_{1}^{2}}=\sigma_{0}^{2}-\frac{\sigma_{0}^{4}}{\sigma_{0}^{2}+\sigma_{1}^{2}}$

因式分解出一個(gè)部分,，這部分用k表示：

相當(dāng)于如果是一維數(shù)據(jù)下，對于之前的預(yù)測值,，僅僅是簡單將兩者疊加相乘就可以得到新的預(yù)測值。

如果是一個(gè)多維矩陣呢,？我們將（12）與（13）表示為矩陣形式,。Σ表示協(xié)方差矩陣， $\bar{u}$ 表示平均向量：

上述的這個(gè)矩陣K 就是卡爾曼增益,。

現(xiàn)在,，我們有兩個(gè)獨(dú)立的維度去估計(jì)系統(tǒng)狀態(tài)：

預(yù)測值

測量值

將兩者相乘帶入（15）尋找他們的重疊區(qū)域：

從（14）可知，卡爾曼增益為

到了關(guān)鍵步驟,！原文是一句帶過,，但為本文的通俗考慮，我多解釋兩句：

將（16）的第一個(gè)公式的兩邊都除以 $H_{k}$ 可得到（18）的第一個(gè)公式（注意,，其中定義了,，所以結(jié)合了），同時(shí)（16）的第二個(gè)公式的左右兩邊都約掉 $H_{K} H_{K}^{T}$ ,，可以得到（18）的第二個(gè)公式（一樣結(jié)合了這個(gè)定義）
然后對（17）的兩邊也都同時(shí)除以 $H_{k}$ （結(jié)合了上面的定義）,，可以得到（19）

以為就這樣了？我對正在讀此文的你提一個(gè)問題,，上面為何要從（16）（17）除以 $H_{k}$ 變到（18）（19）呢,，這就要談到卡爾曼增益的本質(zhì)了?？柭鲆娴谋举|(zhì)其實(shí)就是把（11）里面經(jīng)常用到的一個(gè)中間結(jié)果定義了一個(gè)變量,，它的物理意義在于：

根據(jù)兩方面的方差來決定最終的權(quán)重，這個(gè)相信大家都已理解了,；
把觀測域變回到狀態(tài)域：即（16）是在觀測域,，除以 $H_{k}$ 就可以變回到狀態(tài)域（18）?？赡芰ⅠR就有同學(xué)疑問了,，啥，為啥,？這個(gè)時(shí)候別忘了之前 $H_{k}$ 的定義：狀態(tài)域通過乘以 $H_{k}$ 轉(zhuǎn)換到觀測域,，反過來觀測域除以 $H_{k}$ 則變回到狀態(tài)域,。

2.2.6 全流程回顧：從預(yù)測傳感器噪聲 $Q_{k}$ 到觀測傳感器噪聲 $R_{k}$

至此，原外文針對卡爾曼濾波的過程差不多了,，但為考慮增加本文的可讀性,，所以我再總結(jié)如下：

回顧下整個(gè)過程

最開頭，我們分別建立了系統(tǒng)的協(xié)方差矩陣 $P_{k}$ ,，以及狀態(tài)轉(zhuǎn)移/預(yù)測矩陣 $F_{k}$ ,，預(yù)測傳感器不確定的外部因素描述為協(xié)方差為 $Q_{k}$ 的高斯噪聲；
隨后把預(yù)測狀態(tài)量到觀測量的變換矩陣設(shè)為 $H_{k}$ ,，測量值與真實(shí)值之間的不確定性（即觀測傳感器噪聲）的協(xié)方差設(shè)為 $R_{k}$ ,，讀數(shù)的平均值為 $\bar{z}_{k}$

進(jìn)而求出了卡爾曼增益的計(jì)算公式：

再回到遞推公式（核心思想就是用測量值修正預(yù)測值）

$\hat{\mathbf{x}}_{k}^{\prime}=\hat{\mathbf{x}}_{k}+\mathbf{K}^{\prime}\left(\overrightarrow{\mathbf{z}_{k}}-\mathbf{H}_{k} \hat{\mathbf{x}}_{k}\right)$

從而有

特別大的時(shí)候（測量值相對不靠譜，畢竟R代表傳感器噪聲,，R越大代表噪聲越大,，也就意味著測量值越不靠譜），卡爾曼增益趨于零,，也就是去更多相信預(yù)測值 $\hat{x}_{k}$ ,；
當(dāng)特別小的時(shí)候（測量值相對靠譜），卡爾曼增益就是 ,，從而代入遞推公式,，可得 $\hat{\mathbf{x}}_{k}^{\prime}=\hat{\mathbf{x}}_{k}+\mathbf{K}^{\prime}\left(\overrightarrow{\mathbf{z}_{k}}-\mathbf{H}_{k} \hat{\mathbf{x}}_{k}\right) = K$

也算是再次驗(yàn)證了卡爾曼濾波里的數(shù)據(jù)融合思想。

終于,，我們得到了最佳預(yù)測值 $\tilde{x}_{k}^{\prime}$ ,，可以把它繼續(xù)放進(jìn)去做另一輪預(yù)測，持續(xù)迭代（獨(dú)立于 $P_{k}^{\prime}$ ）,。

以上所有公式,，僅需要實(shí)現(xiàn)（7）（18）（19），這使你可以對任何線性系統(tǒng)建模,。

公式7類似于預(yù)測未來,，公式18、19類似修正當(dāng)下,，簡言之,，則是前者預(yù)測后者更新。

另外,，有一點(diǎn)值得一提的是,，公式18中括號里的部分就是所謂的“更新量”，這個(gè)更新量的物理意義是實(shí)際與期望測量值的誤差,，而卡爾曼增益則是這部分更新量對于預(yù)測值的權(quán)重,。

當(dāng)然，卡爾曼濾波的推導(dǎo)思路很多：1 從加權(quán)的角度,；2 從貝葉斯的角度,；3 從最小二乘角度,。限于篇幅，本文只介紹加權(quán)的角度,，至于后兩者,，讀者可以自行查閱相關(guān)文章，比如參考文獻(xiàn)14的《機(jī)器人學(xué)中的狀態(tài)估計(jì)》,，這本書講得不錯(cuò),，可能相關(guān)符號的表示有所差別（比如本文里的、 $\mathbf{B}_{k} \overrightarrow{\mathbf{u}_{k}}$ ,、 $H_{k}$ ,、 $\bar{z}_{k}$ 在書中分別是、 $v_{k}$ ,、 $C_{k}$ ,、 $y_{k}$ ），但不影響理解,，拍圖如下，最后還可以看下參考文獻(xiàn)18,、19,。

至于非線性系統(tǒng)，則可以使用擴(kuò)展卡爾曼濾波（Extended Kalman Filter,，EKF）,，相當(dāng)于對測量和預(yù)測值的平均值進(jìn)行簡單線性化，下部分會詳細(xì)介紹,。

第三部分卡爾曼濾波的相關(guān)擴(kuò)展算法

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

3.1.1 什么是EKF：KF的非線性擴(kuò)展

還記得前言中說的這句話不：“這算法神到什么程度呢,，比如有人號稱當(dāng)年阿波羅就靠它登陸上的月球 :) ”???? ，哈哈,，實(shí)際上,，此言不虛。

preview

當(dāng)年登月的美國宇航工程師就曾把卡爾曼濾波用于阿波羅計(jì)劃非常的軌跡估計(jì)問題中,，進(jìn)而導(dǎo)出了后面發(fā)展而來的擴(kuò)展卡爾曼濾波器,，具體工作包括

將KF擴(kuò)展到非線性的運(yùn)動(dòng)、觀測模型,；
提出了減小非線性影響的線性化方法,；
將原始的KF重新公式化為目前熟知的標(biāo)準(zhǔn)形式（預(yù)測+更新）

換言之

當(dāng)系統(tǒng)為線性高斯模型（正太分布）時(shí)，卡爾曼濾波器能給出最優(yōu)的估計(jì),，但是實(shí)際系統(tǒng)總是存在不同程度的非線性,，如平方、三角關(guān)系,、開方等,。相當(dāng)于系統(tǒng)的狀態(tài)轉(zhuǎn)移函數(shù)（對應(yīng)狀態(tài)轉(zhuǎn)移矩陣）,，和觀測函數(shù)（對應(yīng)觀測變換矩陣）均為非線性函數(shù)。
對于非線性系統(tǒng),，可以采用的一種方法是通過線性化方法將非線性系統(tǒng)轉(zhuǎn)換為近似的線性系統(tǒng),，即為擴(kuò)展卡爾曼濾波EKF，核心思想是：圍繞濾波值將非線性函數(shù)展開成泰勒級數(shù)并略去二階及以上項(xiàng),，得到一個(gè)近似的線性化模型,，然后應(yīng)用卡爾曼濾波完成狀態(tài)估計(jì)。

3.1.2 EFK的一個(gè)示例：跟蹤汽車運(yùn)動(dòng)軌跡

為形象說明,，我們再看另外一個(gè)例子,，即通過毫米波雷達(dá)跟蹤汽車的運(yùn)動(dòng)軌跡

首先，定義如下變量（圖引自Udacity）

其次,，假定我們把觀測值z與預(yù)測值x'之間的差值記為y,，則可以有

用 $p_{x}$ 、 $p_{y}$ 以及 $v_{x}$ ,、 $v_{y}$ 表示預(yù)測后的位置及速度,，可得

通過一系列的換算可得（中間換算過程可以看參考文獻(xiàn)20，本文限于篇幅則不再把中間換算過程一一列出,，畢竟本部分不是本文重點(diǎn),，了解下思想即可）

從上面的公式很容易看出，等式兩邊的轉(zhuǎn)化是非線性的,，并不存在一個(gè)常數(shù)矩陣H,，能夠使得等式兩邊成立。

如果將高斯分布作為輸入,，輸入到一個(gè)非線性函數(shù)中,，得到的結(jié)果將不再符合高斯分布，也就將導(dǎo)致卡爾曼濾波的公式不再適用,。

因此我們需要將上面的非線性函數(shù)轉(zhuǎn)化為近似的線性函數(shù)求解,。

在大學(xué)課程《高等數(shù)學(xué)》中我們學(xué)過，非線性函數(shù)y=h(x)可通過泰勒公式在點(diǎn) $(x_{0},y_{0})$ 處展開為泰勒級數(shù)：

忽略二次以上的高階項(xiàng),，即可得到近似的線性化方程,，用以替代非線性函數(shù)h(x)，即：

將非線性函數(shù)h(x)拓展到多維,，即求各個(gè)變量的偏導(dǎo)數(shù),，即：

對x求偏導(dǎo)數(shù)所對應(yīng)的這一項(xiàng)被稱為雅可比（Jacobian）式。

雅克比矩陣是函數(shù)的一階偏導(dǎo)數(shù)以一定方式排列成的矩陣,，通俗講則是

一元數(shù)量函數(shù)的導(dǎo)數(shù)是一個(gè)數(shù)量,，即斜率
多元數(shù)量函數(shù)的導(dǎo)數(shù)是一個(gè)向量，即梯度
多元向量值函數(shù)的導(dǎo)數(shù)是一個(gè)矩陣，即 Jacobia

更多請看參考文獻(xiàn)21,，從其發(fā)明歷史步步深入,。

我們將求偏導(dǎo)數(shù)的公式與之前推導(dǎo)的公式對應(yīng)起來看x的系數(shù)，會發(fā)現(xiàn)這里的測量矩陣H其實(shí)就是泰勒公式中的雅可比式,。

多維的雅可比式的推導(dǎo)過程有興趣的同學(xué)可以自己研究一下,，這里我們直接使用其結(jié)論：

求得非線性函數(shù)h(x')對 $p_{x}$ 、 $p_{y}$ ,、 $v_{x}$ ,、 $v_{y}$ 的一階偏導(dǎo)數(shù)，并排列成的矩陣,，最終得到雅克比（Jacobian）矩陣H：

其中

接下來就是考驗(yàn)各位高等數(shù)學(xué)求偏導(dǎo)數(shù)功底的時(shí)候了,！

經(jīng)過一系列計(jì)算，最終得到測量矩陣H為：

根據(jù)以上公式可知,，在每次預(yù)測障礙物的狀態(tài)后,，需要根據(jù)預(yù)測的位置和速度計(jì)算出對應(yīng)的測量矩陣H，這個(gè)測量矩陣為非線性函數(shù)h(x')在x'所在位置進(jìn)行求導(dǎo)的結(jié)果,。

3.2 無跡卡爾曼濾波UKF

既然EKF已經(jīng)能夠很好的解決非線性問題了,，為什么還需要UKF？
原因是現(xiàn)實(shí)生活中存在很多完全不規(guī)律的分布,，我們需要更好的近似方式和更高的性能,。讓我們來看下在EKF中如何解決非線性問題的（下圖來自參考文獻(xiàn)15，巧的是作者William還是湖南老鄉(xiāng) + 微信好友）：

可以看到原始高斯分布經(jīng)過非線性變換之后其實(shí)是一個(gè)不規(guī)則的非線性分布,，在EKF中我們在高斯均值點(diǎn)附近用泰勒級數(shù)近似非線性分布，從而獲得近似高斯分布,。但是問題在于,，這個(gè)近似高斯分布有時(shí)不是足夠精確，單單一個(gè)均值點(diǎn)往往無法預(yù)測上圖中這種很不規(guī)則的分布的,。這個(gè)時(shí)候我們就需要無跡卡爾曼濾波UKF了,。

UKF從原始高斯分布中提取包括均值點(diǎn)在內(nèi)的一系列代表點(diǎn)，并將代表點(diǎn)帶入到非線性等式中,，圍繞這些點(diǎn)進(jìn)行近似,，從而獲得更好的近似結(jié)果。

在上圖中我們可以看到,，藍(lán)色橢圓代表的UKF近似高斯分布更接近真實(shí)的不規(guī)則分布,。
總的來說，與EKF相比,，UKF是處理非線性過程模型或非線性測量模型的更好的替代方法,。UKF不會對非線性函數(shù)進(jìn)行線性化處理，它用所謂的sigma 點(diǎn)來近似概率分布。其優(yōu)勢在于在很多情況下,，sigma 點(diǎn)近似非線性轉(zhuǎn)換的效果比線性化更好,，此外它還可以省去計(jì)算復(fù)雜的雅可比矩陣的過程。更多可以查看參考文獻(xiàn)16,。

參考文獻(xiàn)與擴(kuò)展閱讀

智能網(wǎng)聯(lián)汽車技術(shù)路線圖2.0及創(chuàng)新應(yīng)用路線圖
如何入門學(xué)習(xí)自動(dòng)駕駛技術(shù)（一）??????
七月在線《無人駕駛實(shí)戰(zhàn)》小課
Wikipedia上關(guān)于卡爾曼濾波的解釋
從貝葉斯方法談到貝葉斯網(wǎng)絡(luò)
如何通俗理解隱馬爾可夫模型HMM？
卡爾曼濾波通俗易懂的解釋
關(guān)于卡爾曼濾波的經(jīng)典外文：《How a Kalman filter works, in pictures》
這是上文的其中一個(gè)翻譯版：如何通俗并盡可能詳細(xì)地解釋卡爾曼濾波,？
這是上文的另外一個(gè)翻譯版：圖說卡爾曼濾波,，一份通俗易懂的教程,，這兩個(gè)翻譯版都有各自的一些小問題，當(dāng)然,，本文已避免
《概率機(jī)器人 [Probabilistic robotics]》,，Google無人駕駛創(chuàng)始人Sebastian Thrun等人所著，本書權(quán)威且推導(dǎo)全面但不夠通俗
《視覺SLAM十四講：從理論到實(shí)踐（第2版）》,，挺不錯(cuò)的一本書,，結(jié)合七月SLAM課足夠掌握SLAM了
卡爾曼增益推導(dǎo)
《機(jī)器人學(xué)中的狀態(tài)估計(jì)》，狀態(tài)估計(jì)方向的必讀書,，相比《概率機(jī)器人》更通俗些,，而且有意思的是，這本書的譯者就是視覺SLAM十四講的作者高翔等人
SLAM基礎(chǔ)-擴(kuò)展卡爾曼濾波??????
自動(dòng)駕駛感知融合-無跡卡爾曼濾波(Lidar&Radar)
數(shù)據(jù)挖掘中所需的概率論與數(shù)理統(tǒng)計(jì)知識
貝葉斯視角下的卡爾曼濾波
從最小二乘到Kalman濾波
通俗易懂理解——卡爾曼濾波與擴(kuò)展卡爾曼濾波
雅可比矩陣幾何意義的直觀解釋及應(yīng)用
卡爾曼濾波器實(shí)現(xiàn)詳解

后記

總算成文了,，坦誠講,，我在寫本篇博客之前雖然看過很多文章、資料,，但在動(dòng)筆之前并沒有對卡爾曼濾波及其相關(guān)算法有足夠清晰透徹的理解,，是邊寫邊清晰的。

包括中秋三天一直在完善本文,，寫博十年以來,，心中總有一個(gè)執(zhí)念，那就是要讓沒有學(xué)過計(jì)算機(jī)的也能看懂我寫的文章（否則何必寫）,，所以會特別注重這兩點(diǎn)：

多圖,、多舉例
在他人文章中那些一步到位的推導(dǎo)，在我這,，能拆成10步則必會拆成10步

因?yàn)橹挥凶銐蛲ㄋ滓锥?，才可以讓更多初學(xué)者更快入門，才能讓我更有成就感,、滿足感,，當(dāng)然，歡迎大家不吝批評指正，最后,，更深入的可以看下七月在線相關(guān)的課,。

以下是本文的修改記錄（開寫后的一周內(nèi)，基本一天一改進(jìn)/深入）：

21年9.19,，改進(jìn)1.1節(jié),，特別是無人駕駛軟件架構(gòu)的六大模塊；
21年9.20,，改進(jìn)2.1節(jié) 什么是卡爾曼濾波,；
21年9.21，改進(jìn)2.2節(jié),，主要是卡爾曼增益的推導(dǎo)
21年9.22,，再次改進(jìn)2.2節(jié)，主要是卡爾曼增益的物理意義,；
21年9.23,，再次改進(jìn)2.2節(jié)，主要總結(jié)卡爾曼濾波的三種推導(dǎo)思路,，以及優(yōu)化3.1節(jié) 擴(kuò)展卡爾曼濾波EKF
21年10.10,，七月曉老師幫完善和修正1.1節(jié)的相關(guān)細(xì)節(jié)
21年10.11，七月曉老師幫完善預(yù)測傳感器噪聲協(xié)方差 $Q_{k}$ 和觀測傳感器噪聲協(xié)方差 $R_{k}$ 的選取依據(jù),，具體來說則是 $Q_{k}$ 根據(jù)預(yù)測的傳感器噪聲來定（比如imu或者輪速計(jì)）,， $R_{k}$ 根據(jù)觀測傳感器的噪聲來定（比如相機(jī)、lidar）
23年3.17日,，根據(jù)本文評論下兩位讀者的反饋,，修正相關(guān)描述，從而變?yōu)椋骸凹尤胪獠恳蛩貐f(xié)方差后,，擴(kuò)大后的紫色高斯分布和原來沒有外部因素的小范圍紫色高斯分布相比,，兩者之間有相同的均值，但協(xié)方差不同”

本站是提供個(gè)人知識管理的網(wǎng)絡(luò)存儲空間,，所有內(nèi)容均由用戶發(fā)布，不代表本站觀點(diǎn),。請注意甄別內(nèi)容中的聯(lián)系方式,、誘導(dǎo)購買等信息，謹(jǐn)防詐騙,。如發(fā)現(xiàn)有害或侵權(quán)內(nèi)容,，請點(diǎn)擊一鍵舉報(bào)。

轉(zhuǎn)藏 分享

QQ空間 QQ好友新浪微博微信

獻(xiàn)花（0） +1

來自： netouch > 《我的圖書館》

舉報(bào)/認(rèn)領(lǐng)