Kalman濾波及其在SLAM中應(yīng)用ppt課件

1、康康 丹丹Kalman濾波簡(jiǎn)單實(shí)例擴(kuò)展kalman濾波SLAM問(wèn)題 狀態(tài)空間模型：在控制領(lǐng)域中，用輸入向量、狀態(tài)向量和輸出向量描述系統(tǒng)的一種方法，叫狀態(tài)空間模型。如下式 表示 時(shí)刻的狀態(tài)向量， 表示 時(shí)刻的輸入向量， 表示 時(shí)刻的輸出向量， A表示狀態(tài)矩陣，B為控制矩陣或者輸入矩陣，C為觀測(cè)矩陣，D為輸入輸出矩陣。(1)( )( )( )( )( )x kAx kBu ky kCx kDu k)(kx)(ku)(kykkk 背景介紹： Kalman，匈牙利數(shù)學(xué)家。 卡爾曼濾波器源于他的博士論文和1960年發(fā)表的論文A New Approach to Linear Filtering and P

2、rediction Problems（線性濾波與預(yù)測(cè)問(wèn)題的新方法）。 引入一個(gè)離散控制系統(tǒng)的控制模型 ： 定義系統(tǒng)的觀測(cè)變量為z，得到測(cè)量方程為： 其中， 表示 時(shí)刻的狀態(tài)向量， 表示 時(shí)刻的輸入向量， 表示 時(shí)刻的觀測(cè)向量， 分別為過(guò)程激勵(lì)噪聲和觀測(cè)噪聲，它們?yōu)橄嗷オ?dú)立、正態(tài)分布的白噪聲，協(xié)方差分別是Q，R這里假設(shè)它們不隨系統(tǒng)狀態(tài)變化而變化） )(kx)()()() 1(kwkBukAxkx)()()(kvkHxkzk)(kuk)(kzk)()(kvkw、 kalman濾波第一步： 用系統(tǒng)的過(guò)程模型來(lái)預(yù)測(cè)下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是 ，可以預(yù)測(cè)下一時(shí)刻k+1出現(xiàn)的狀態(tài)： 更新?tīng)顟B(tài) 的

3、協(xié)方差矩陣： 其中， 為過(guò)程激勵(lì)噪聲協(xié)方差。(1| )( | )( )x kkAx k kBu k)| 1(kkx)(kxQAkkAPkkPT)|()| 1(Q 結(jié)合觀測(cè)值更新?tīng)顟B(tài)估計(jì)，得到k+1時(shí)刻狀態(tài)為： 其中， 稱為新息。 為kalman增益，其計(jì)算公式為： 式中， 為觀測(cè)噪聲協(xié)方差。)| 1() 1()| 1() 1| 1(kkHxkzKkkxkkxggK)| 1()| 1(RHkkHPHkkPKTTg)| 1() 1() 1(kkHxkzkVR 為了令kalman不斷迭代下去，需要更新K時(shí)刻狀態(tài) 的協(xié)方差： 時(shí)間更新和測(cè)量更新不斷重復(fù)迭代，就是kalman最吸引人的特性。 ) 1|

4、1(kkx)| 1()() 1| 1(kkPHKIkkPg)| 1()() 1| 1() 3()| 1() 1()| 1() 1| 1() 1()2()| 1()| 1(11kkPHKIkkPkkHxkzKkkxkkxkzRHkkHPHkkPKkalmanggTTg更新測(cè)量誤差更新估計(jì)由觀測(cè)變量增益）計(jì)算（測(cè)量更新（校正）QAkkAPkkPkBukkAxkkxT)|()| 1()2()()|()| 1() 1 (向前推算誤差協(xié)方差向前推算狀態(tài)變量時(shí)間更新（預(yù)測(cè)）為初始估計(jì)和)|()|(kkPkkx 考慮房間內(nèi)溫度對(duì)象過(guò)程: 即過(guò)程的狀態(tài)不隨時(shí)間變化，沒(méi)有控制輸入；包含噪聲的觀測(cè)值是狀態(tài)變量的直

5、接體現(xiàn)。 那么，kalman濾波的5個(gè)方程分別為： (1)( )( )( )( )( )x kx kw kz kx kv k)| 1()() 1| 1()| 1() 1()| 1() 1| 1()| 1()(| 1()|()| 1()|()| 1(1kkPKIkkPkkxkzKkkxkkxRkkPkkPKQkkPkkPkkxkkxggg 假設(shè)房間溫度不變，k時(shí)刻為23度估計(jì)值），該值的協(xié)方差假設(shè)為5度上一時(shí)刻更新的協(xié)方差為3度，噪聲為2度）。 房間內(nèi)有一溫度計(jì)，其值顯示25度，該值的偏差噪聲為2度。 則kalman增益為： K+1時(shí)刻的真實(shí)估計(jì)值為： 該時(shí)刻更新協(xié)方差為即為上邊3度的計(jì)算式）

6、71. 0)25/(5gK42.24)2325(*71. 023) 1| 1(kkx45. 15)71. 01 () 1| 1(kkP 可以看出，卡爾曼濾波估計(jì)是一個(gè)線性隨機(jī)系統(tǒng)的狀態(tài)。然而實(shí)際中，很多系統(tǒng)是非線性的，處理這些系統(tǒng)時(shí)，用擴(kuò)展卡爾曼濾波(EKF),它是將期望和方差線性化的卡爾曼濾波器。 控制對(duì)象的狀態(tài)空間表述為: )(),(),() 1(kwkukxfkx)(),()(kvkxhkz 擴(kuò)展卡爾曼濾波的5個(gè)方程為：)| 1()() 1| 1() 3()| 1() 1()| 1() 1| 1() 1()2()| 1()| 1(11kkPHKIkkPkkxhkzKkkxkkxkzVRV

7、HkkHPHkkPKkalmanggTTTg更新測(cè)量誤差更新估計(jì)由觀測(cè)變量增益）計(jì)算（測(cè)量更新（校正）TTWQWAkkAPkkPkukkxfkkx)|()| 1()2()0),(),|()| 1() 1 (向前推算誤差協(xié)方差向前推算狀態(tài)變量時(shí)間更新（預(yù)測(cè)）為初始估計(jì)和)|()|(kkPkkxA是f對(duì)x的偏導(dǎo)的雅可比矩陣，即W是f對(duì)w的偏導(dǎo)的雅可比矩陣，即H是h對(duì)x的偏導(dǎo)的雅可比矩陣，即V是h對(duì)v的偏導(dǎo)的雅可比矩陣，即 ,iijixfA,iijiwhH,iijiwfW,iijivhV SLAM：Simultaneous localization and mapping自主定位和地圖創(chuàng)建。有時(shí)也稱

8、為CMLconcurrent mapping and localization） 問(wèn)題陳述：自主移動(dòng)機(jī)器人從未知環(huán)境中一個(gè)未知點(diǎn)開(kāi)始運(yùn)動(dòng)，經(jīng)過(guò)一系列未知特征點(diǎn)，并得到這些點(diǎn)到機(jī)器人距離的一組測(cè)量數(shù)據(jù)。這組測(cè)量數(shù)據(jù)由機(jī)器人頭部的測(cè)距傳感器測(cè)得。 SLAM處理的目標(biāo)是根據(jù)這些測(cè)量數(shù)據(jù)得到環(huán)境的估計(jì)地圖，并用這張地圖實(shí)現(xiàn)機(jī)器人的定位。 在SLAM中，機(jī)器人運(yùn)動(dòng)模型已知，即運(yùn)動(dòng)角速度和線速度，可以估計(jì)機(jī)器人的位置；機(jī)器人與特征點(diǎn)之間的測(cè)量值可以測(cè)出，即知道測(cè)量值。在SLAM中，根據(jù)EKF，用每一狀態(tài)的測(cè)量值來(lái)更新機(jī)器人的運(yùn)動(dòng)位置，然后由機(jī)器人位置和測(cè)量值求出特征點(diǎn)的坐標(biāo)。 K時(shí)刻，機(jī)器人狀態(tài)可表示為

9、Xv(k)表示機(jī)器人的狀態(tài)，Xlmk為路標(biāo)的狀態(tài)，它不隨時(shí)間變化而變化,可以寫成Xlm。 觀測(cè)值是描述機(jī)器人和特征點(diǎn)距離的函數(shù)，那么 其中， 為特征點(diǎn)i與機(jī)器人之間的距離， 為機(jī)器人與特征點(diǎn)位置的夾角。 )(ki )()()()()()( )()(kykxXkkykxkXkXkXXlmlmlmTVehlmv其中，TiiikkkZ)(),()()(ki EKF-SLAM算法： (1) 初始化機(jī)器人的位置Xv(0)和協(xié)方差陣P(0)，可以令其都為0. (2) 預(yù)測(cè) (3) 觀測(cè)值 路標(biāo)特征值的預(yù)測(cè)值的新息為 新息的誤差協(xié)方差矩陣為： )()|()| 1()(),|()| 1(kQAkkAPkkPk

10、ukkXfkkXTVV)| 1() 1() 1(kkxhkzkVRHkkHPkST)| 1() 1( (4)數(shù)據(jù)關(guān)聯(lián) 檢驗(yàn)k+1時(shí)觀測(cè)到的特征點(diǎn)與0,1,k時(shí)觀測(cè)到的特征點(diǎn)是否關(guān)聯(lián)上，根據(jù)下式證明時(shí)刻觀測(cè)到的一組特征點(diǎn)確實(shí)是觀測(cè)到的，其中G為常數(shù)；否則，舍棄觀測(cè)值。 (5)更新 GkVkSkVT) 1() 1() 1()|1()() 1|1()|1() 1()|1() 1|1()|1()|1(1kkPHKIkkPkkxhkzKkkxkkxRHkkHPHkkPKkalmanggTTg協(xié)方差矩陣更新機(jī)器人狀態(tài)：增益 (6)前往(2)遞歸執(zhí)行(2)-(5). 該算法在matlab上仿真執(zhí)行結(jié)果為: 目前問(wèn)題： EKF-SL