基于無跡卡爾曼濾波的SLAM算法
目的：
求解SLAM問題時一般將移動機器人的位姿狀態(tài)和環(huán)境地圖中的路標(biāo)位置組合成聯(lián)合的系統(tǒng)狀態(tài)向量，并利用SLAM算法對聯(lián)合系統(tǒng)狀態(tài)向量作最小均方誤差估計托酸，并把移動機器人在移動過程中新觀測到的路標(biāo)向量加入到聯(lián)合狀態(tài)向量离咐，以此來逐步構(gòu)建環(huán)境地圖并同時更新機器人在環(huán)境中的位置孵滞。本文要研究的問題是通過采用無跡卡爾曼濾波算法來提高SLAM定位估計的精度。
SLAM問題的UKF實現(xiàn)：
在matlab上實現(xiàn)SLAM仿真，在一張自定義地圖中放置多個路標(biāo)點档玻，設(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ù)測值夯到∪络裕可用和分別表示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ǔ)上改進算法來進一步提高定位的精度修噪。
