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

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

3、狀態(tài)變量的直接體現(xiàn)。然后，卡爾曼濾波的五個(gè)方程如下：簡(jiǎn)單的例子，假設(shè)室溫是恒定的，k時(shí)間是23度(估計(jì)值)，并且這個(gè)值的協(xié)方差被假設(shè)為5度(在前一時(shí)間更新的協(xié)方差是3度，并且噪聲是2度)。房間里有一個(gè)溫度計(jì)，它的值顯示25度，這個(gè)值的偏差(噪音)是2度。然后卡爾曼增益為：K 1處的實(shí)際估計(jì)值為：此時(shí)的更新協(xié)方差為(即，高3度的計(jì)算公式)，卡爾曼濾波器被擴(kuò)展。可以看出，卡爾曼濾波器估計(jì)線性隨機(jī)系統(tǒng)的狀態(tài)。然而，在實(shí)踐中，許多系統(tǒng)是非線性的。當(dāng)處理這些系統(tǒng)時(shí)，使用擴(kuò)展卡爾曼濾波器(EKF)，它使期望和方差線性化。控制對(duì)象的狀態(tài)空間表示為：擴(kuò)展卡爾曼濾波器有五個(gè)方程：擴(kuò)展卡爾曼濾波器，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問題， 有時(shí)也被稱為CML(并發(fā)映射和定位)問題陳述：自主移動(dòng)機(jī)器人從未知環(huán)境中的未知點(diǎn)移動(dòng)，經(jīng)過一系列未知特征點(diǎn)，并獲得從這些點(diǎn)到機(jī)器人的距離的一組測(cè)量數(shù)據(jù)。 這組測(cè)量數(shù)據(jù)由機(jī)器人頭部的測(cè)距傳感器測(cè)量。SLAM處理的目標(biāo)是根據(jù)這些測(cè)量數(shù)據(jù)得到估計(jì)的環(huán)境地圖，并利用該地圖實(shí)現(xiàn)機(jī)器人定位。SLAM問題，在SLAM中，機(jī)器人的運(yùn)動(dòng)模型是已知的，即運(yùn)動(dòng)的角速度和線速度可以估計(jì)機(jī)器人的位置；可以測(cè)量機(jī)器人和特征點(diǎn)之間的測(cè)量值，也就是說，可以知道測(cè)量值。在SLAM中，根據(jù)EKF，

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