基于無跡卡爾曼濾波的SLAM算法

目的：

求解SLAM問題時一般將移動機器人的位姿狀態(tài)和環(huán)境地圖中的路標位置組合成聯(lián)合的系統(tǒng)狀態(tài)向量，并利用SLAM算法對聯(lián)合系統(tǒng)狀態(tài)向量作最小均方誤差估計，并把移動機器人在移動過程中新觀測到的路標向量加入到聯(lián)合狀態(tài)向量，以此來逐步構(gòu)建環(huán)境地圖并同時更新機器人在環(huán)境中的位置。本文要研究的問題是通過采用無跡卡爾曼濾波算法來提高SLAM定位估計的精度。

SLAM問題的UKF實現(xiàn)：

在matlab上實現(xiàn)SLAM仿真，在一張自定義地圖中放置多個路標點，設(shè)置小車的速度以及傳感器能夠檢測的范圍，求出系統(tǒng)非線性狀態(tài)方程和非線性觀測方程。初始化，根據(jù)K時刻狀態(tài)估計值 和協(xié)方差矩陣對n維狀態(tài)向量采樣獲得2n+1個Sigma點。]時間預(yù)測，通過非線性模型對Sigma點的狀態(tài)進行預(yù)測，利用這些Sigma點集的狀態(tài)值加權(quán)求和得到均值和協(xié)方差預(yù)測值?？捎煤头謩e表示Sigma點的均值和協(xié)方差對應(yīng)的權(quán)值。

觀測更新，通過非線性的觀測方程對觀測量的均值和協(xié)方差進行預(yù)測，通過加權(quán)求和得到。?????

狀態(tài)更新，利用K+1時刻獲得的觀測值對狀態(tài)變量進行更新，定義新息，濾波增益矩陣，更新狀態(tài)和協(xié)方差。

目前進展:

在相同的地圖下，分別對EKF-SLAM和UKF-SLAM仿真作了比較，由于計算量較大的原因會導(dǎo)致計算機運行速度變慢，所以每隔500個計數(shù)點取一次數(shù)據(jù)求取誤差均值，并求取34個點，并計算兩種算法的所用時間。結(jié)果證明，UKF的誤差均值只有1.815m，EKF的誤差均值有3.548m，UKF算法明顯優(yōu)于EKF算法，運行時間UKF用了182秒，而EKF用了212秒，運行時間也明顯減短了。

總結(jié)：

因為相對于擴展卡爾曼算法，無跡卡爾曼算法不需要對非線性系統(tǒng)的線性化過程忽略高階項，也不需要獲得Jacobian矩陣，采用UT變化能提高定位的精度。實驗結(jié)果無跡卡爾曼算法計算速度和定位平均誤差都會減小。所以可以針對于UKF的基礎(chǔ)上改進算法來進一步提高定位的精度。