卡爾曼濾波視頻學(xué)習(xí)記錄
資源來(lái)源:Michel van Biezen SPECIAL TOPICS THE KALMAN FILTER
官網(wǎng):http://www.ilectureonline.com/lectures/subject/SPECIAL%20TOPICS/26/190
嗶哩嗶哩:https://www.bilibili.com/video/av36187842/?p=1
該視頻教程簡(jiǎn)單妈拌、易懂敦冬,是入門級(jí)很不錯(cuò)的教程耍铜!
附帶一個(gè)可以免費(fèi)翻墻的瀏覽器 鏈接:https://pan.baidu.com/s/1Uip__jFc0CFZIpAfU2tGMg 提取碼:fu1u
Lecture 1: What is A Kalman Filter?
Definition: it is an iterative mathematical process, use a set of equations and consecutive data inputs to quickly estimate the true value, position, velocity, etc of the object being measured.When the measure values contain unpredicted or random error, uncertainty or variation.
解釋:卡爾曼濾波是一種對(duì)感興趣的信號(hào)進(jìn)行迭代更新的過(guò)程专筷,它利用幾個(gè)公式結(jié)合當(dāng)前測(cè)量值(一般具有測(cè)量誤差谍咆、隨機(jī)誤差等特性)的輸入扮碧,對(duì)感興趣的信號(hào)的真實(shí)值進(jìn)行快速蟋滴、最優(yōu)估計(jì)粹庞。
例子:
上圖是利用KF對(duì)溫度計(jì)測(cè)量的溫度進(jìn)行濾波浩淘,溫度計(jì)由于自身的誤差原因捌朴,得到的溫度并不是真實(shí)值,會(huì)出現(xiàn)上下波動(dòng)张抄,如圖中的離散點(diǎn)砂蔽。真實(shí)值理論上上一條水平的直線,此處利用KF來(lái)對(duì)溫度測(cè)量值進(jìn)行濾波欣鳖,可以快速地逼近真實(shí)值這條曲線察皇。
通常KF的應(yīng)用主要是融合多個(gè)測(cè)量值,如在定位方面的有GPS泽台、IMU什荣、雷達(dá)等手段獲取定位結(jié)果,這幾種手段各有優(yōu)缺點(diǎn)怀酷,此時(shí)采用KF將其進(jìn)行融合處理稻爬,使其優(yōu)勢(shì)互補(bǔ),增強(qiáng)系統(tǒng)的測(cè)量精度以及魯棒性蜕依。
Lecture 2: Flowchart of A Simple Example
上圖是卡爾曼濾波的簡(jiǎn)易版框圖桅锄,KF的主要流程為(1) Calculate Gain, (2) Calculate Current Estimate, (3) Calculate New Estimate Error. 每一次迭代都是進(jìn)行上述三個(gè)流程,從而獲得當(dāng)前的最優(yōu)估計(jì)(Update Estimate)样眠。
(1)計(jì)算誤差增益:對(duì)預(yù)測(cè)誤差和測(cè)量數(shù)據(jù)誤差進(jìn)行一種權(quán)衡友瘤。簡(jiǎn)單來(lái)說(shuō),增益代表的就是更加相信那個(gè)檐束,誰(shuí)的誤差小辫秧,就多相信些,加權(quán)的時(shí)候比例更大被丧。
(2)更新當(dāng)前估計(jì):利用預(yù)測(cè)值和測(cè)量值盟戏,結(jié)合第一步得到的增益來(lái)估計(jì)當(dāng)前的值绪妹。
(3)更新預(yù)測(cè)誤差:利用第二步的更新值以及第一步的增益值計(jì)算下一步的預(yù)測(cè)誤差。
備注:Original Error Estimate初值一般取大一些柿究,這樣剛開(kāi)始的時(shí)候會(huì)比較相信測(cè)量值邮旷,保證預(yù)測(cè)不會(huì)太過(guò)偏離真實(shí)值,后面更新迭代過(guò)程中會(huì)隨著時(shí)間逐漸改變蝇摸。
Lecture 3: The Kalman Gain: A Closer Look
如圖所示為卡爾曼增益KG定義以及在迭代公式中的作用婶肩。
KG定義為預(yù)測(cè)誤差與預(yù)測(cè)+測(cè)量誤差的比例,范圍在0-1之間波動(dòng)探入。
如圖中的KG趨勢(shì)圖所示狡孔,當(dāng)KG為1時(shí),表明測(cè)量誤差很小蜂嗽,比較精確苗膝,此時(shí)我們選擇更加相信測(cè)量值,即在更新公式后面部分比較重要植旧,作為當(dāng)前時(shí)刻的更新增量辱揭。反之,KG為0時(shí)病附,表示預(yù)測(cè)誤差很小问窃,預(yù)測(cè)值比較穩(wěn)定,此時(shí)更加相信預(yù)測(cè)值完沪。
備注:理論上KG會(huì)來(lái)越來(lái)小域庇,因?yàn)镵F會(huì)快速收斂,接近真實(shí)值覆积。
Lecture 4: The 3 Calculations of The Kalman Filter
如圖為迭代更新過(guò)程中的三個(gè)階段對(duì)應(yīng)的三個(gè)公式听皿。
第一個(gè)公式為KG計(jì)算方法,表示測(cè)量和預(yù)測(cè)誤差的比例宽档,KG越大表明預(yù)測(cè)誤差越大尉姨。
第二個(gè)公式為更新?tīng)顟B(tài)的,根據(jù)前一時(shí)刻的預(yù)測(cè)值結(jié)合當(dāng)前時(shí)刻的增量來(lái)更新當(dāng)前時(shí)刻的狀態(tài)吗冤。
第三個(gè)公式為預(yù)測(cè)誤差更新又厉,利用當(dāng)前時(shí)刻的測(cè)量誤差和前一時(shí)刻的預(yù)測(cè)誤差來(lái)計(jì)算,常用后面那個(gè)推導(dǎo)的更新公式椎瘟。(這個(gè)公式來(lái)源覆致?推導(dǎo)可參考秦永元的《卡爾曼濾波與組合導(dǎo)航原理》)
備注:KG越小,此時(shí)測(cè)預(yù)誤差比較大肺蔚,預(yù)測(cè)誤差更新越緩慢篷朵,保證逐步收斂,不出現(xiàn)跳躍。
Lecture 5: A Simple Example of The Kalman Filter Numerical temperature example
下圖是一個(gè)溫度估計(jì)的例子声旺,展現(xiàn)了KF的三個(gè)階段的計(jì)算過(guò)程。
反復(fù)迭代以上過(guò)程段只,預(yù)測(cè)誤差會(huì)越來(lái)越小腮猖。
Lecture 6: A Simple Example of The Kalman Filter(Continued)
下圖是一個(gè)更加詳細(xì)的推導(dǎo)例子,從圖中的推導(dǎo)過(guò)程可以看出赞枕,經(jīng)過(guò)4次迭代后澈缺,預(yù)測(cè)值由68變化到71,逐漸接近真實(shí)值72炕婶〗闵模可以看出,KF的收斂速度是比較快的柠掂。
Lecture 7: The Multi-Dimension Model 1
下圖為KF的具體流程公式项滑,首先給定一個(gè)狀態(tài)和預(yù)測(cè)誤差矩陣初值,然后進(jìn)入圖中央的迭代循環(huán)過(guò)程涯贞。
第一步(PREVIOUS STATE):將當(dāng)前時(shí)刻的更新為前一時(shí)刻的狀態(tài)和誤差枪狂。
第二步(NEW STATE(PREDICTED)):利用系統(tǒng)模型更新當(dāng)前時(shí)刻的預(yù)測(cè)狀態(tài)矩陣和預(yù)測(cè)誤差矩陣,其中A宋渔、B都是系數(shù)矩陣州疾,為控制方差矩陣,為預(yù)測(cè)狀態(tài)噪聲矩陣皇拣,Q為處理噪聲的協(xié)方差矩陣严蓖。上述系數(shù)、矩陣的含義以及在實(shí)際中如何選取Question氧急?颗胡??常用運(yùn)動(dòng)學(xué)模型?
第三步(UPDATE MEASUREMENT AND KALMAN GAIN):計(jì)算KF增益K以及根據(jù)第二步的預(yù)測(cè)值和當(dāng)前時(shí)刻的測(cè)量值進(jìn)行融合得到當(dāng)前時(shí)刻的最優(yōu)估計(jì)狀態(tài)态蒂。其中測(cè)量值此時(shí)的系數(shù)矩陣C代表預(yù)測(cè)值與測(cè)量值之間的轉(zhuǎn)換關(guān)系杭措,實(shí)際應(yīng)用中如何體現(xiàn)? (兩個(gè)例子 1.和等效 狀態(tài)矩陣可以直接測(cè)量 2.通過(guò)某種關(guān)系建立)
第四步(CURRENT BECOMES PREVIOUS):進(jìn)行預(yù)測(cè)誤差矩陣更新,輸出當(dāng)前最優(yōu)估計(jì)結(jié)果钾恢。
備注:實(shí)際運(yùn)用中RTKLIB開(kāi)源庫(kù)預(yù)測(cè)方程常常是用上一次的定位結(jié)果手素,量測(cè)方程需要根據(jù)實(shí)際情況建立。
Lecture 8: The Multi-Dimension Model 2 –The State Matrix
狀態(tài)矩陣指的是我們感興趣的瘩蚪、想要估計(jì)的參數(shù)泉懦,其形式通常如,并且通過(guò)預(yù)測(cè)(狀態(tài))方程來(lái)更新疹瘦。其中為外加控制變量矩陣崩哩,為過(guò)程噪聲矩陣,其更新周期一般越小越好(RTKLIB定位里直接用上一秒的定位結(jié)果,原因可能是太小邓嘹,即當(dāng)前時(shí)刻和前一時(shí)刻位置一致)酣栈。實(shí)際應(yīng)用過(guò)程中如何建立預(yù)測(cè)方程?動(dòng)力學(xué)方程
Lecture 9: The Multi-Dimension Model 3 – The State Matrix
該節(jié)舉了三個(gè)例子汹押,根據(jù)動(dòng)力學(xué)定律建立預(yù)測(cè)方程和狀態(tài)矩陣X矿筝,系數(shù)矩陣A一樣,因此其更新結(jié)果也類似(此時(shí)不考慮后面的外加控制變量矩陣以及噪聲矩陣)棚贾。
Lecture 10: The Multi-Dimension Model 4 –The State Matrix
此時(shí)考慮外加控制變量矩陣窖维,那么不同場(chǎng)景其系數(shù)矩陣B會(huì)發(fā)生變化。如何選擇B妙痹?分清楚狀態(tài)矩陣和控制變量矩陣uk的關(guān)系铸史?動(dòng)力學(xué)方程建立
Lecture 11: 5: Find The State Matrix Of A Falling Object
下圖以一個(gè)FALLING OBJECT 為例子,對(duì)狀態(tài)矩陣Xk的更新進(jìn)行了詳細(xì)的推導(dǎo)怯伊。
Lecture 12: 6: Update The State Matrix
下圖展示了觀測(cè)量如何通過(guò)測(cè)量方程與狀態(tài)矩陣建立聯(lián)系的琳轿,圖中系數(shù)矩陣C取值有點(diǎn)奇怪, 觀測(cè)量直接獲得。實(shí)際應(yīng)用中需要仔細(xì)考慮這個(gè)方程如何從抽象的問(wèn)題中建立震贵。
Lecture 13: 7: State Matrix Of Moving Object In 2-D
下圖展示了一個(gè)移動(dòng)物體的二維狀態(tài)矩陣的系統(tǒng)狀態(tài)預(yù)測(cè)過(guò)程(不涉及)
Lecture 14: 8: What Is The Control Variable Matrix?
在上述例子的基礎(chǔ)上利赋,考慮在預(yù)測(cè)方程中。
Lecture 15: 9: Converting From Previous To Current State 2-D
結(jié)合上述兩個(gè)例子猩系,將預(yù)測(cè)方程的各個(gè)部分放在一起媚送,合成2D運(yùn)動(dòng)物體的系統(tǒng)狀態(tài)預(yù)測(cè)方程的實(shí)際應(yīng)用。
Lecture 16: 10: Converting From Previous To Current State 3-D
三維狀態(tài)矩陣的預(yù)測(cè)方程例子寇甸,具體表達(dá)形式如下:
Lecture 17: 11: Numerical Ex. Of Finding The State Matrix 1-D
下圖展示了一個(gè)具體的例子塘偎,給出初值,計(jì)算迭代的結(jié)果拿霉。
Lecture 18: What Is A Covariance Matrix?
下圖主要講述了和的推導(dǎo)吟秩。協(xié)方差矩陣有兩種,狀態(tài)協(xié)方差矩陣P和測(cè)量協(xié)方差矩陣R绽淘,可以理解為誤差矩陣涵防。它們可以推導(dǎo)出KALMAN增益K來(lái)對(duì)預(yù)測(cè)和量測(cè)二者進(jìn)行權(quán)衡(更相信誰(shuí))。
備注:狀態(tài)協(xié)方差矩陣P中存在過(guò)程噪聲矩陣Q沪铭,是為了防止P的更新出現(xiàn)零的特殊情況壮池,其次實(shí)際情況中也會(huì)有噪聲。but 如何取值杀怠?椰憋??經(jīng)驗(yàn)值
Lecture 19: What Is A Variance-Covariance Matrix?
下圖定義了均值赔退、方差橙依、協(xié)方差等的基本概念证舟,從1D、2D到3D協(xié)方差矩陣的模板窗骑。
Lecture 20: Example Of Covariance Matrixn And Standard Deviation
方差表示其離平均值的波動(dòng)范圍女责,也代表測(cè)量值與真實(shí)值的誤差。從高斯模型可以看出慧域,覆蓋數(shù)據(jù)的概率越來(lái)越大鲤竹。
Lecture 21: Finding The Covariance Matrix, Numerical Ex. 1
下圖給出了一個(gè)計(jì)算協(xié)方差矩陣的實(shí)際例子,此處只有主對(duì)角線上方差的計(jì)算:
Lecture 22: Finding The Covariance Matrix, Numerical Ex. 2
下圖計(jì)算為計(jì)算協(xié)方差的實(shí)例昔榴,接上圖,形成完整的協(xié)方差矩陣碘橘。
Lecture 23: Finding The Covariance Matrix, Numerical Example
一個(gè)計(jì)算測(cè)量協(xié)方差矩陣的例子互订,一種技巧-以矩陣操作的思維來(lái)計(jì)算協(xié)方差矩陣。
備注:協(xié)方差矩陣的元素大小表明-->正相關(guān) 同向變化 比如數(shù)學(xué)與物理成正相關(guān)痘拆,數(shù)學(xué)成績(jī)高 物理也高仰禽。
Lecture 24: Finding The State Covariance Matrix: P=?
狀態(tài)協(xié)方差矩陣的簡(jiǎn)單例子,此時(shí)副對(duì)角元素計(jì)算為 纺蛆,這是一個(gè)特殊情況 互相關(guān)系數(shù)為1吐葵,兩個(gè)狀態(tài)線性相關(guān)。
Lecture 25: Explaining The State Covariance Matrix
下圖解釋了狀態(tài)協(xié)方差矩陣代表的含義-主對(duì)角線為估計(jì)狀態(tài)的誤差桥氏,副對(duì)角線為狀態(tài)的相關(guān)性温峭,為0以為二者相互獨(dú)立。
Lecture 26: Flow Chart Of 2-D Kalman Filter - Tracking Airplane
下圖為追蹤飛機(jī)的例子字支,介紹了基本的流程框圖 給定飛機(jī)的位置凤藏、速度、加速度堕伪、測(cè)量值揖庄、各種誤差等。
備注:C如何惹反啤蹄梢? 第五步?-->實(shí)際問(wèn)題
Lecture 27: 1. The Predicted State -Tracking Airplane
第一步 根據(jù)運(yùn)動(dòng)規(guī)律 進(jìn)行預(yù)測(cè)方程建立 迭代更新第一次
Lecture 28: 2. Initial Process Covariance- Tracking Airplane
下圖為狀態(tài)(過(guò)程)誤差矩陣的計(jì)算富俄,實(shí)際應(yīng)用中副對(duì)角線上的元素通常簡(jiǎn)化為0禁炒,因?yàn)榇藭r(shí)這兩個(gè)狀態(tài)之間沒(méi)什么聯(lián)系,我們主要關(guān)注方差(主對(duì)角線元素)蛙酪,它代表了狀態(tài)的誤差齐苛。
Lecture 29: 3. Predicted Process Covariance - Tracking Airplane
下圖根據(jù)初值,計(jì)算更新過(guò)程誤差矩陣Pk桂塞,Q此時(shí)默認(rèn)取0凹蜂。
備注:具體含義?maybe need推導(dǎo)
Lecture 30: 4. Calculate The Kalman Gain - Tracking Airplane
下圖為計(jì)算KALMAN濾波的增益K,實(shí)例中轉(zhuǎn)換矩陣H取單位陣玛痊。其計(jì)算結(jié)果表明此次更新K = 0.405比較小汰瘫,測(cè)量誤差較大(25>20),選擇更加相信預(yù)測(cè)值擂煞。
備注:轉(zhuǎn)換矩陣代表將誤差矩陣P轉(zhuǎn)換到增益矩陣K(物理意義混弥?)。
Lecture 31: 5. The New Observation -Tracking Airplane
下圖為測(cè)量方程的例子对省,此例中直接觀察出狀態(tài)量蝗拿,因此系數(shù)矩陣C為單位陣,觀測(cè)噪聲矩陣取0蒿涎。
備注:實(shí)際問(wèn)題中預(yù)測(cè)方程的模型究竟如何建立哀托?有兩個(gè)例子 1.直接用上一秒的結(jié)果 2.動(dòng)力學(xué)方程預(yù)測(cè)
如何保證其迭代后預(yù)測(cè)更加準(zhǔn)確?高斯函數(shù)G(x)性質(zhì) 兩個(gè)G(x)相乘劳秋,其方差會(huì)變小仓手,即誤差變小。
其次玻淑,實(shí)際中的觀測(cè)量可能是間接量嗽冒,需要建立關(guān)系。
Lecture 32: 6. Calculate Current State -Tracking Airplane
下圖表明了如何更新當(dāng)前狀態(tài)矩陣补履,公式如下添坊,當(dāng)K比較大時(shí),會(huì)有更多的增量
來(lái)更新當(dāng)前的最優(yōu)估計(jì)干像,從結(jié)果可以看出帅腌,的值比較偏向預(yù)測(cè)值。
Lecture 33: 7. Update Process Covariance - Tracking Airplane
下圖表明了更新過(guò)程協(xié)方差矩陣
Lecture 34: 8. Current Becomes Previous -Tracking Airplane
下圖為獲取當(dāng)前的最優(yōu)估計(jì)后麻汰,把當(dāng)前狀態(tài)更新為上一秒狀態(tài)速客,為下一步預(yù)測(cè)做準(zhǔn)備。
Lecture 35: 1, 2, 3 Of Second Iteration -Tracking Airplane
下圖為第二次迭代的 預(yù)測(cè)系統(tǒng)狀態(tài)方程和過(guò)程協(xié)方差矩陣的更新五鲫。
Lecture 36: 4. Kalman Gain Second Iteration - Tracking Airplane
下圖計(jì)算第二次迭代過(guò)程中的KALAM增益矩陣K的計(jì)算溺职。
備注:K為一個(gè)對(duì)角陣。
Lecture 37: 5, 6 Of Second Iteration - Tracking Airplane
下圖為第二次迭代更新過(guò)程中位喂,利用預(yù)測(cè)值和觀測(cè)值結(jié)合KALMAN增益來(lái)更新當(dāng)前狀態(tài)矩陣浪耘。
Lecture 38: 7, 8 Of Second Iteration - Tracking Airplane
第二次迭代過(guò)程 更新?tīng)顟B(tài)誤差矩陣,并將其和狀態(tài)矩陣更新為上一秒數(shù)據(jù)塑崖,為一步迭代做準(zhǔn)備七冲。
Lecture 39: Part 1 Of Third Iteration - Tracking Airplane
第三次迭代更新過(guò)程 獲取當(dāng)前的狀態(tài)預(yù)測(cè)矩陣狀態(tài)協(xié)方差矩陣KALMAN增益。
Lecture 40: Part 2 Of Third Iteration - Tracking Airplane
下圖為第三次迭代過(guò)程中的當(dāng)前最優(yōu)狀態(tài)估計(jì)計(jì)算過(guò)程规婆,并更新當(dāng)前的最優(yōu)狀態(tài)協(xié)方差矩陣澜躺,為下一次迭代做準(zhǔn)備蝉稳。
Lecture 41: Graphing 1St 3 Iterations (T Vs X) - Tracking Airplane
下圖為上述例子的時(shí)間和位移的圖形化表示,從圖中我們可以看出KALMAN濾波就是在預(yù)測(cè)值與測(cè)量值之間根據(jù)誤差大小進(jìn)行折中處理掘鄙。
Lecture 42: Graphing 1St 3 Iterations (T Vs V) - Tracking Airplane
下圖為速度與時(shí)間的關(guān)系耘戚,同上
備注:KALMAN濾波值的輸出是最大概率上符合真實(shí)值的,因此經(jīng)過(guò)三輪迭代后的位移和速度的結(jié)果都比較準(zhǔn)確操漠。