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

1、卡爾曼濾波及其在SLAM中的應(yīng)用，康丹、提綱、卡爾曼濾波的簡單例子，擴展卡爾曼濾波SLAM問題，準備知識，狀態(tài)空間模型：在控制領(lǐng)域，用輸入向量、狀態(tài)向量和輸出向量描述系統(tǒng)的方法稱為狀態(tài)空間模型。下面的等式表示時間上的狀態(tài)向量、時間上的輸入向量和時間上的輸出向量。a代表狀態(tài)矩陣，B代表控制矩陣或輸入矩陣，C代表觀察矩陣，D代表輸入和輸出矩陣?？柭鼮V波器，背景介紹：卡爾曼，匈牙利數(shù)學(xué)家?？柭鼮V波源于他的博士論文和1960年發(fā)表的線性濾波和預(yù)測問題的新方法。介紹了一種離散控制系統(tǒng)的控制模型：將系統(tǒng)的觀測變量定義為Z，得到測量方程：表示時間的狀態(tài)向量、表示時間的輸入向量和表示時間的觀測向量分別為過

2、程激勵噪聲和觀測噪聲，它們是獨立的正態(tài)分布的白噪聲，協(xié)方差分別為Q，R(這里， 假設(shè)它們不隨系統(tǒng)狀態(tài)的改變而改變)，卡爾曼濾波器，假設(shè)當前系統(tǒng)狀態(tài)是，則有可能在下一時刻預(yù)測k 1的狀態(tài)：更新狀態(tài)的協(xié)方差矩陣：其中它是過程激勵噪聲的協(xié)方差?？柭鼮V波器，結(jié)合觀測值來更新狀態(tài)估計，k 1處的狀態(tài)被獲得如下：其中，它被稱為新息。是卡爾曼增益，它的計算公式是：其中是觀測噪聲的協(xié)方差。為了使卡爾曼連續(xù)迭代，需要在k時刻更新狀態(tài)協(xié)方差：時間更新和測量更新是重復(fù)迭代的，這是卡爾曼最吸引人的特性。舉一個簡單的例子，考慮房間中的溫度對象過程：也就是說，過程的狀態(tài)不隨時間改變，并且沒有控制輸入；含有噪聲的觀測值是

3、狀態(tài)變量的直接體現(xiàn)。然后，卡爾曼濾波的五個方程如下：簡單的例子，假設(shè)室溫是恒定的，k時間是23度(估計值)，并且這個值的協(xié)方差被假設(shè)為5度(在前一時間更新的協(xié)方差是3度，并且噪聲是2度)。房間里有一個溫度計，它的值顯示25度，這個值的偏差(噪音)是2度。然后卡爾曼增益為：K 1處的實際估計值為：此時的更新協(xié)方差為(即，高3度的計算公式)，卡爾曼濾波器被擴展?？梢钥闯觯柭鼮V波器估計線性隨機系統(tǒng)的狀態(tài)。然而，在實踐中，許多系統(tǒng)是非線性的。當處理這些系統(tǒng)時，使用擴展卡爾曼濾波器(EKF)，它使期望和方差線性化?？刂茖ο蟮臓顟B(tài)空間表示為：擴展卡爾曼濾波器有五個方程：擴展卡爾曼濾波器，A是F到X的偏

4、導(dǎo)數(shù)的雅可比矩陣，即W是F到W的偏導(dǎo)數(shù)的雅可比矩陣，即H到X的偏導(dǎo)數(shù)的雅可比矩陣，即V是H到V的偏導(dǎo)數(shù)的雅可比矩陣，即SLAM問題， 有時也被稱為CML(并發(fā)映射和定位)問題陳述：自主移動機器人從未知環(huán)境中的未知點移動，經(jīng)過一系列未知特征點，并獲得從這些點到機器人的距離的一組測量數(shù)據(jù)。 這組測量數(shù)據(jù)由機器人頭部的測距傳感器測量。SLAM處理的目標是根據(jù)這些測量數(shù)據(jù)得到估計的環(huán)境地圖，并利用該地圖實現(xiàn)機器人定位。SLAM問題，在SLAM中，機器人的運動模型是已知的，即運動的角速度和線速度可以估計機器人的位置；可以測量機器人和特征點之間的測量值，也就是說，可以知道測量值。在SLAM中，根據(jù)EKF，

5、機器人的運動位置用每個狀態(tài)的測量值更新，然后從機器人的位置和測量值獲得特征點的坐標。SLAM問題，在時間k，機器人狀態(tài)可以表示為Xv(k)，表示機器人狀態(tài)，而Xlm(k)，不隨時間變化，可以寫成Xlm。觀察值是描述機器人和特征點之間的距離的函數(shù)，其中它是特征點1和機器人之間的距離以及機器人和特征點之間的夾角。EKF-SLAM算法：(1)初始化機器人的位置Xv(0)和協(xié)方差矩陣P(0)，它們都可以是0。(2)預(yù)測(3)觀測標志特征值預(yù)測值的新息誤差協(xié)方差矩陣為：0，1，k SLAM問題，(4)觀測特征點與數(shù)據(jù)關(guān)聯(lián)檢驗中的k 1否則，觀測值被丟棄。(5)更新，SLAM問題，(6)返回，(2)遞歸執(zhí)行，(2)-(5)