對(duì)xuslamdemo理解-p黃嘉明

SLAM的定義KF原理EKF原理EKF-SLAM簡(jiǎn)介Xu

EKF

SLAM代碼的主要架構(gòu)SLAM:即時(shí)定位及地圖構(gòu)建；就是指機(jī)器人不僅要知道在所處環(huán)境中的位置，還有記錄環(huán)境中的特征點(diǎn)用來(lái)創(chuàng)建或更新地圖。由于機(jī)器在移動(dòng)的過(guò)程中每次定位都會(huì)有誤差，誤差多次累積就會(huì)帶來(lái)更大的誤差?？梢酝ㄟ^(guò)移動(dòng)時(shí)路標(biāo)特征值得匹配來(lái)達(dá)到減少定位誤差的目的。KF原理

KF有一個(gè)狀態(tài)（過(guò)程）模型和觀測(cè)模型，并且兩個(gè)模型都是線性變化。其中各個(gè)狀

態(tài)變量x、控制變量u、觀測(cè)變量z及過(guò)程噪聲w和觀測(cè)噪聲v都應(yīng)該符合高斯分布，這即時(shí)

所說(shuō)的線性卡爾曼。過(guò)程方程：觀測(cè)更新方程：?可以通過(guò)狀態(tài)模型利用t時(shí)刻的后驗(yàn)估計(jì)得到t時(shí)刻的先驗(yàn)估計(jì)(通過(guò)X^(t|t)得到X^（t+1|t）)，再利用協(xié)方差公式得到先驗(yàn)估計(jì)誤差協(xié)方差P（t+1|t）x?(t

1|

t)

~上式利用高斯分布的性質(zhì)可得x?

t

t1)(|

F?

t

t

Bt

w

t)---先驗(yàn)估計(jì)期望：先驗(yàn)估計(jì)協(xié)方差：注：P(t+1|t)=

xt1通過(guò)過(guò)程模型得到狀態(tài)有誤差，但是觀測(cè)值也存在誤差，所以

不能單獨(dú)的利用過(guò)程模型或者觀測(cè)模型去估計(jì)在t時(shí)刻的狀態(tài)；而卡爾曼將t時(shí)刻的估計(jì)量定義成（1）式；把他倆加個(gè)權(quán)，求個(gè)和，那就可能更準(zhǔn)確了。Hxtxxkttttt1t

1(|(1)z)(

?

11)(|??定義：Y

Z

(t

1)

Z?(t

1|

t)為新息，即觀測(cè)變量與其

之差忽略觀測(cè)噪聲時(shí)所以

比較（1）式可以得到新息為Y

Z

(t

1)

Hx?(t

1|

t)新息的方差用S表示為：z?(t

1|

t)

Hx?(t

1|

t)S

E[YYT

]其中后驗(yàn)估計(jì)誤差為（2）式~x

(t

1|

t

1)

x(t

1)

-

x?(t

1|

t

1).................(2)后驗(yàn)估計(jì)誤差協(xié)方差為（3）式E[~x(t

1|

t

1)~x(t

1|

t

1)T

]..........(3)后驗(yàn)估計(jì)誤差協(xié)方差最小時(shí)就可以求出k:這就是卡爾曼增益。最后

就可以得出后驗(yàn)估計(jì)誤差協(xié)方差。最終求得滿足條件的卡爾曼增益K為:最后

得到后驗(yàn)估計(jì)誤差協(xié)方差P(t+1|t+1)P(t+1|t+1)=時(shí)間

更新：為下一時(shí)刻t+1狀態(tài)提供了提供了先驗(yàn)估計(jì)

x?(t

1|

t);且提供誤差協(xié)方差

P(k+1|k)這就為測(cè)量更新時(shí)求卡曼增益K；最終得到后驗(yàn)估計(jì)。而后驗(yàn)估計(jì)為計(jì)算下一時(shí)刻t+2狀態(tài)的計(jì)算先驗(yàn)估計(jì)提供這個(gè)時(shí)刻t+1的后驗(yàn)估計(jì)x?

(t

1

|

t

1),以及后驗(yàn)估計(jì)誤差的協(xié)方差。PredictionUpdateInnovation

/

ResidualInnovation

varianceKalman

gainLinear

Kalman

filterEKF原理因?yàn)樯厦娴腒F使用要求要求系統(tǒng)的兩個(gè)模型應(yīng)該是線性的，即使

在兩個(gè)模型中考慮了過(guò)程噪聲和觀測(cè)噪聲也難以滿足實(shí)際應(yīng)用的要求，通過(guò)一定的方法使卡曼濾波具有現(xiàn)實(shí)意義。

可以通過(guò)對(duì)過(guò)程和觀測(cè)模型進(jìn)行泰勒展開(kāi)得到其一階展開(kāi)式，使之與LKF類似，便于實(shí)際應(yīng)用，這就是擴(kuò)展型卡爾曼濾波EKF對(duì)于在泰勒展開(kāi)中的求導(dǎo)需要使用雅克比行列式。主要使用的公式和流程如下圖：需要注意的是在狀態(tài)方程泰勒展開(kāi)時(shí)是以X?

(t

|

t)的期望為展開(kāi)點(diǎn)，而觀測(cè)方程是以x?(t

1|

t)的期望為展開(kāi)點(diǎn)。EKF-SLAM簡(jiǎn)介SLAM問(wèn)題主要為求解后驗(yàn)概率分布即P(XK|ZK,UK),值得注意的是此時(shí)XK聯(lián)合狀態(tài)向量為[XT

，MT]TVK

K

vk

kX

和M

分別表示機(jī)器人位姿和地圖信息的隨機(jī)量。利用貝葉斯概率和馬可夫原則SLAM的概率模型如下圖圖SLAM的概率模型當(dāng)觀測(cè)量和機(jī)器人的狀態(tài)向量及其初始狀

態(tài)均服從高斯分布時(shí)，可以用均值和協(xié)方

差表征高斯分布，此時(shí)SLAM

問(wèn)題就轉(zhuǎn)換為估算p(Xk|Zk，Uｋ)的均均值和協(xié)方差PK的過(guò)程。EKF和EKF-SLAM的主要不同之一在于：EKF根據(jù)觀測(cè)量，只對(duì)機(jī)器人的狀態(tài)進(jìn)行實(shí)時(shí)修正，而SLAM要不斷從外界獲取觀測(cè)信息，并同時(shí)

機(jī)器人和地圖的狀態(tài)。下圖為EKF-SLAM算法。圖EKF-SLAM算法SLAM主要包括

過(guò)程、特征值提取及數(shù)據(jù)融合、觀測(cè)更新過(guò)程、增廣過(guò)程。對(duì)于特征值提取和數(shù)據(jù)融合需先從觀測(cè)量中提取特征值，提取特征點(diǎn)后

要判定觀測(cè)量是否與當(dāng)前的狀態(tài)向量中的特征點(diǎn)存在關(guān)聯(lián)關(guān)系，諾存在則狀態(tài)更新，否則判定是否為新特征點(diǎn)，如果是就加

合狀態(tài)向量中否則舍棄。Xu

EKF

SLAM代碼主要架構(gòu)Filter文件為其

代碼其中包括

、觀測(cè)更新、增廣等。這部分代碼運(yùn)行時(shí)首先要調(diào)用vehicleModel函數(shù)文件通過(guò)控制量（角度、速度等）根據(jù)運(yùn)動(dòng)模型得到其最初始位置X0和控制量U。然后在移動(dòng)的過(guò)程中每次進(jìn)行定位要進(jìn)行如下步驟:①

通過(guò)conNoise函數(shù)添加控制噪聲（仿真模擬實(shí)際情況）后，再調(diào)用predictEKF函數(shù)根據(jù)前一時(shí)刻的位置估計(jì)x?(t

1|

t

1)得到

x?(t

|

t

1)和協(xié)方差P（t|t-1）；②實(shí)際觀測(cè)應(yīng)用機(jī)器人自身攜帶的傳感器獲取特路標(biāo)i，利用getLandmark函數(shù)實(shí)現(xiàn)得相對(duì)對(duì)傳感器路標(biāo)點(diǎn)的測(cè)量Zi；利用觀測(cè)

obsModel獲得其路標(biāo)觀測(cè)估計(jì)且利用obsNoise添加觀測(cè)噪聲；③

如果開(kāi)關(guān)openNN為零則運(yùn)行correspond函數(shù)，此時(shí)觀測(cè)量和目標(biāo)點(diǎn)的對(duì)應(yīng)關(guān)系已知；在地圖中存在與當(dāng)前觀測(cè)量對(duì)應(yīng)的目標(biāo)點(diǎn)，將此觀測(cè)量加入到已有特征點(diǎn)(old

feature)行列；若不存在與之有對(duì)應(yīng)關(guān)系的目標(biāo)點(diǎn)，則將這類觀測(cè)量加入到新特征點(diǎn)(newfeature)

中如果開(kāi)關(guān)openNN非零應(yīng)用最

關(guān)聯(lián)函數(shù)nearestNeighbour對(duì)實(shí)際測(cè)量值進(jìn)行匹配，建立建立特征點(diǎn)索引的過(guò)程。通過(guò)調(diào)用函數(shù)obsModel獲取觀測(cè)值z(mì)及其雅克比行列式，近而求出其新息，卡爾曼增益等。④如果開(kāi)關(guān)openIEKF非零運(yùn)行IEKF-SLAM且openEKF的值表示迭代觀測(cè)更新的次數(shù)；否則運(yùn)行EKF-SLAM，使用用updateEKF函數(shù)應(yīng)用上面得到的卡爾曼增益修正x?(t

|

t

1)得到當(dāng)前機(jī)器人的估計(jì)位置x?(t

|