基于中心差分卡爾曼濾波器的快速SLAM算法

田 翔，張 亮，陳耀武

(浙江大學(xué)數(shù)字技術(shù)及儀器研究所，浙江杭州310027，cyw@mail.bme.zju.edu.cn)

移動(dòng)機(jī)器人同時(shí)定位與地圖創(chuàng)建 SLAM (Simultaneous Localization and Mapping)問題［1－2］是指在未知環(huán)境中移動(dòng)機(jī)器人利用自身定位信息創(chuàng)建環(huán)境地圖，同時(shí)利用已創(chuàng)建的環(huán)境地圖進(jìn)行自身定位.經(jīng)過十多年的研究和發(fā)展已經(jīng)產(chǎn)生了許多SLAM算法，典型的包括基于擴(kuò)展卡爾曼濾波的SLAM算法(EKF-SLAM)，基于粒子濾波器的SLAM算法(FASTSLAM1、FASTSLAM2)，基于稀疏信息矩陣濾波器的SLAM算法(EIF-SLAM)等［3－8］.由于在EKF-SLAM中狀態(tài)向量和狀態(tài)向量協(xié)方差都需要維護(hù)所有已檢測(cè)的路標(biāo)，因此在路標(biāo)密集環(huán)境下SLAM速度很慢，從而限制其在實(shí)際中的應(yīng)用.另外由于機(jī)器人SLAM中的預(yù)測(cè)過程和測(cè)量過程都是非線性模型，所以在EKFSLAM算法中使用了一級(jí)泰勒展開中對(duì)預(yù)測(cè)過程和測(cè)量過程進(jìn)行線性近似，但是當(dāng)非線性方程的非線性嚴(yán)重時(shí)會(huì)導(dǎo)致EKF-SLAM算法的一致性出現(xiàn)問題［9］.在FASTSLAM中SLAM的速度和一致性受粒子數(shù)目的影響，粒子數(shù)目過小可能導(dǎo)致SLAM結(jié)果發(fā)散，粒子數(shù)目過多會(huì)導(dǎo)致SLAM速度太慢從而限制其在實(shí)際中的應(yīng)用.

本文提出基于中心差分卡爾曼濾波器的快速SLAM算法對(duì)預(yù)測(cè)過程和測(cè)量過程的線性近似精度上進(jìn)行了改進(jìn)，使用Stirling插值方法可以對(duì)非線性過程近似到二級(jí)泰勒展開，從而提高SLAM的精確性，并且使用已檢測(cè)過的路標(biāo)統(tǒng)計(jì)信息對(duì)由已檢測(cè)路標(biāo)和機(jī)器人位置組成的狀態(tài)向量和狀態(tài)協(xié)方差矩陣進(jìn)行動(dòng)態(tài)調(diào)整，以保證它們的大小限制在固定范圍內(nèi)，從而提高了SLAM速度.本文算法同EKF-SLAM和FASTSLAM進(jìn)行分析發(fā)現(xiàn)在內(nèi)存占用上要優(yōu)于EKF-SLAM和FASTSLAM.通過對(duì)在稀疏路標(biāo)環(huán)境和密集路標(biāo)環(huán)境下的SLAM結(jié)果進(jìn)行比較表明在SLAM精確性和一致性上都優(yōu)于FASTSLAM2.0算法.

1 中心差分卡爾曼濾波器SLAM

中心差分卡爾曼濾波器是一種對(duì)非線性過程使用Stirling多項(xiàng)式插值公式近似，同時(shí)將其和卡爾曼濾波器結(jié)合所組成的濾波器.基于此濾波器對(duì)移動(dòng)機(jī)器人進(jìn)行SLAM稱為CDKFSLAM.

1.1 Stirling多項(xiàng)式插值

式中:u和δ分別為操作符.

對(duì)于式(1)如果只考慮到二級(jí)近似而忽略之后的所有高階項(xiàng)則可以簡(jiǎn)化為

式(2)中操作符u和δ的定義為

對(duì)式(2)中的操作符使用式(3)進(jìn)行替換可得

對(duì)比式(5)和對(duì)f(x)在ˉx處的泰勒展開可以看出使用Stirling插值公式對(duì)非線性方程f的近似可以達(dá)到二級(jí)泰勒展開，同時(shí)選取合適的參數(shù)h還可以近似到更高級(jí)別的泰勒展開.

當(dāng)表達(dá)式(1)中的輸入隨機(jī)變量x為多維變量時(shí)，式(2)可以重新被表示為

式(7)中操作符up和δp為

式中:ep為第p項(xiàng)元素為1其余項(xiàng)元素為0的N維向量.

式(6)中輸入隨機(jī)變量x經(jīng)過非線性過程f后輸出的均值為

式中:sp為輸入變量x的協(xié)方差Cholesky系數(shù)的第p項(xiàng).

式(6)中輸入隨機(jī)變量x經(jīng)過非線性過程f后輸出的協(xié)方差以及輸出和輸入x之間的互協(xié)方差分別為

綜合考慮式(9)、式(11)以及式(12)可以看出對(duì)非線性方程使用Stirling插值公式的二級(jí)展開近似后，非線性方程的輸出均值、協(xié)方差以及輸出和輸入的互協(xié)方差完全由2N+1個(gè)輸入點(diǎn)經(jīng)過非線性方程f后的輸出決定.這些輸入點(diǎn)統(tǒng)稱為Sigma點(diǎn)［12－13］.結(jié)合式(9)、式(11)和式(12)同卡爾曼濾波器組成中心差分卡爾曼濾波器，并使用此濾波器對(duì)SLAM中的狀態(tài)預(yù)測(cè)方程，測(cè)量方程進(jìn)行估計(jì).

1.2 SLAM狀態(tài)向量預(yù)測(cè)

在CDKFSLAM過程中，狀態(tài)向量和狀態(tài)向量協(xié)方差分別為

狀態(tài)向量預(yù)測(cè)是指根據(jù)機(jī)器人當(dāng)前狀態(tài)向量SV，同時(shí)參考機(jī)器人速度和以及噪聲參數(shù)預(yù)測(cè)機(jī)器人下一時(shí)刻的位置.狀態(tài)向量預(yù)測(cè)方程為

式中:V為機(jī)器人的運(yùn)動(dòng)速度，Δt為時(shí)刻k和時(shí)刻k+1的時(shí)間間隔，Gn為方位角噪聲，WB為機(jī)器人的輪間距.

由式(15)可知為了根據(jù)k時(shí)刻的狀態(tài)向量預(yù)測(cè)k+1時(shí)刻的狀態(tài)向量需要使用k時(shí)刻的狀態(tài)向量SVk，機(jī)器人速度參數(shù)V，機(jī)器人方位角噪聲參數(shù)，時(shí)間間隔以及機(jī)器人輪間距.由于機(jī)器人速度和方位角參數(shù)受噪聲影響，因此在使用Stirling多項(xiàng)式插值方法對(duì)式(15)進(jìn)行非線性近似時(shí)對(duì)輸入Sigma點(diǎn)的構(gòu)造需要考慮機(jī)器人速度和角速度噪聲.使用式(9)和式(11)對(duì)狀態(tài)向量的均值和協(xié)方差預(yù)測(cè)時(shí)的輸入Sigma點(diǎn)來(lái)進(jìn)行選擇

式中:V為機(jī)器人移動(dòng)速度參數(shù)，Vn為機(jī)器人移動(dòng)速度噪聲均值，Gn為機(jī)器人方位角噪聲均值，VV為機(jī)器人速度噪聲協(xié)方差，GV為方位角噪聲協(xié)方差.由于機(jī)器人移動(dòng)速度參數(shù)和機(jī)器人方位角噪聲參數(shù)的維數(shù)都為1，所以式(18)中SN=3+ 2n+1+1，輸入Sigma點(diǎn)的總數(shù)為2SN+1，在這里機(jī)器人移動(dòng)速度噪聲和方位角噪聲都近似認(rèn)為符合高斯分布，因此參數(shù)h取值為

將式(18)中各個(gè)輸入向量代入式(15)可以獲得各個(gè)輸入向量經(jīng)過狀態(tài)向量預(yù)測(cè)方程的輸出，同時(shí)結(jié)合式(9)和式(11)獲得預(yù)測(cè)狀態(tài)向量和其協(xié)方差

1.3 SLAM狀態(tài)向量更新

在機(jī)器人運(yùn)動(dòng)過程中接收到傳感器測(cè)量數(shù)據(jù)時(shí)先根據(jù)Stirling插值方式對(duì)測(cè)量方程的輸出進(jìn)行估計(jì)，再使用卡爾曼濾波器對(duì)狀態(tài)向量更新，測(cè)量方程為

同樣在使用Stirling插值方式對(duì)測(cè)量方程輸出進(jìn)行估計(jì)時(shí)需要考慮到測(cè)量過程的距離測(cè)量噪聲和方位角測(cè)量噪聲，因此此時(shí)輸入Sigma點(diǎn)的選擇為

式中:Dn為距離測(cè)量噪聲的均值，An為方位角測(cè)量噪聲的均值，DV為距離測(cè)量噪聲的協(xié)方差，AV為方位角測(cè)量噪聲的協(xié)方差;由于DV和AV的維數(shù)都為1，所以式(22)中的SM=3+2n+1+1，輸入Sigma點(diǎn)總數(shù)為2SM+1，參數(shù)h取值為

在測(cè)量數(shù)據(jù)過程中，由于距離測(cè)量噪聲和方位角測(cè)量噪聲與狀態(tài)向量SV互不相關(guān)并且測(cè)量式(19)的輸出即為機(jī)器人和路標(biāo)之間的相對(duì)距離和相對(duì)方位角，所以對(duì)測(cè)量方程的輸入Sigma點(diǎn)的選取可以簡(jiǎn)化為

同時(shí)對(duì)均值和協(xié)方差的估計(jì)需要由式(9)、式(11)修正為

式中:Noise.mean為噪聲均值所組成的向量，Noise.cov為噪聲協(xié)方差所組成的協(xié)方差矩陣.從式(25)和式(26)可以看出此時(shí)噪聲參數(shù)對(duì)于非線性輸出的估計(jì)可以認(rèn)為是單純的附加量.

相對(duì)于使用式(20)～式(22)對(duì)測(cè)量方程輸出的估計(jì)使用式(23)～式(28)時(shí)輸入Sigma點(diǎn)數(shù)目從2(3+2n+1+1)+1降低為2(3+2n)+ 1，從而降低了時(shí)間占用量.當(dāng)使用式(23)～式(28)獲得對(duì)第i個(gè)路標(biāo)的測(cè)量數(shù)據(jù)向量MVk+1和測(cè)量數(shù)據(jù)向量協(xié)方差矩陣 PMVk+1后，根據(jù)式(12)獲取測(cè)量方程的輸入變量和測(cè)量方程輸出MVk+1的協(xié)方差為

1.4 狀態(tài)向量及狀態(tài)協(xié)方差動(dòng)態(tài)調(diào)整

由狀態(tài)向量和狀態(tài)向量協(xié)方差矩陣可知隨著已檢測(cè)到路標(biāo)數(shù)目的增大狀態(tài)向量SV和狀態(tài)向量協(xié)方差PV的維數(shù)都會(huì)逐漸增大，這樣會(huì)嚴(yán)重影響SLAM的速度.因此根據(jù)已測(cè)量到的路標(biāo)統(tǒng)計(jì)信息提出一種動(dòng)態(tài)狀態(tài)向量和狀態(tài)向量協(xié)方差調(diào)整的方法.假設(shè)k時(shí)刻之前所檢測(cè)到的路標(biāo)索引序列為

式中:MLi為在第i時(shí)刻所檢測(cè)到的路標(biāo)索引序列.根據(jù)檢測(cè)路標(biāo)統(tǒng)計(jì)信息計(jì)算每個(gè)路標(biāo)對(duì)于當(dāng)前時(shí)刻狀態(tài)向量的影響權(quán)重，同時(shí)依據(jù)此權(quán)重對(duì)當(dāng)前狀態(tài)向量和狀態(tài)向量協(xié)方差實(shí)現(xiàn)動(dòng)態(tài)調(diào)整.

為簡(jiǎn)單起見，在k時(shí)刻計(jì)算各個(gè)路標(biāo)權(quán)重時(shí)只考慮到k之前的m個(gè)時(shí)刻的路標(biāo)統(tǒng)計(jì)信息，m取值范圍為1～k，距離當(dāng)前時(shí)刻k越近的路標(biāo)索引序列所對(duì)應(yīng)的權(quán)重系數(shù)越大.這樣k時(shí)刻所參考的路標(biāo)索引序列為

每次路標(biāo)索引的權(quán)重系數(shù)滿足wk＞wk－1＞… ＞wk－m+2＞wk－m+1，權(quán)重系數(shù)為

在時(shí)刻k各個(gè)路標(biāo)的權(quán)重為

式中:i為路標(biāo)索引.

式中:當(dāng)路標(biāo)i包括在檢測(cè)路標(biāo)索引序列MLj中時(shí)其值為1，否則為0.

為將狀態(tài)向量SV和狀態(tài)向量協(xié)方差矩陣PV控制在固定大小內(nèi)，定義閾值MAXLM，MAXLM為在SV和PV中所包含的最大路標(biāo)數(shù).當(dāng)SV和PV中路標(biāo)數(shù)目大于MAXLM時(shí)首先通過式(35)獲取每個(gè)路標(biāo)在此時(shí)刻的權(quán)重，然后在SV和PV中找到權(quán)重最小的路標(biāo)并將其從SV和PV中刪除，直至SV和PV中所包含路標(biāo)數(shù)目小于等于MAXLM.當(dāng)在后續(xù)時(shí)刻重新檢測(cè)到被刪除過的路標(biāo)時(shí)將其作為未被檢測(cè)過的路標(biāo)對(duì)待.

1.5 初始狀態(tài)及其協(xié)方差確定

由于在初始狀態(tài)時(shí)還未檢測(cè)到任何路標(biāo)，因此初始狀態(tài)向量SV1和狀態(tài)向量協(xié)方差PV1中只包括機(jī)器人位置信息.

SV1和PV1組成的式(37)滿足自由度為3的卡方分布，根據(jù)一致性理論［14］可知當(dāng)式(37)的值位于雙邊概率為95%的區(qū)域時(shí)認(rèn)為其符合一致性要求.由卡方分布可知此時(shí)雙邊概率區(qū)域?yàn)椋?.22 9.35］.

根據(jù)上述要求，當(dāng)速度噪聲滿足N～(0，VV)，方位角噪聲滿足N～(0，AV)時(shí)，初始狀態(tài)向量和狀態(tài)向量協(xié)方差取值為

此時(shí)式(38)和式(39)滿足一致性要求.

2 內(nèi)存分析

在CDKFSLAM算法中所占用的內(nèi)存由狀態(tài)向量SV，狀態(tài)向量協(xié)方差PV以及參考路標(biāo)序列MLref組成.當(dāng)環(huán)境中路標(biāo)總體數(shù)目為L(zhǎng)N時(shí)，CDKFSLAM占用的最大內(nèi)存為

式中:(3+2MAXLM)為狀態(tài)向量SV大小，(3+ 2MAXLM)2為狀態(tài)向量協(xié)方差PV大小，mLN為參考路標(biāo)序列大小.

EKFSLAM算法中內(nèi)存占用量由狀態(tài)向量和狀態(tài)向量協(xié)方差組成，其占用最大內(nèi)存為

在FASTSLAM中，內(nèi)存占用量和粒子數(shù)目有關(guān).每個(gè)粒子所占用內(nèi)存由機(jī)器人位置向量，機(jī)器人位置協(xié)方差，各個(gè)路標(biāo)位置向量以及各個(gè)路標(biāo)位置協(xié)方差組成.假設(shè)粒子數(shù)目為PMAX，其占用最大內(nèi)存為

式中:(3+32)為機(jī)器人位置向量和位置向量協(xié)方差大小，(2+22)為每個(gè)路標(biāo)位置向量和位置向量協(xié)方差大小.

對(duì)比式(40)～式(42)可知CDKFSLAM和FASTSLAM的內(nèi)存占用和環(huán)境中路標(biāo)總數(shù)LN成線性關(guān)系，EKFSLAM的內(nèi)存占用和LN成二次型關(guān)系.圖1描述了 CDKFSLAM、EKFSLAM和FASTSLAM的內(nèi)存占用情況隨路標(biāo)數(shù)目LN變化的關(guān)系，其中，m取值為10.從圖1中可以看出CDKFSLAM對(duì)內(nèi)存的占用要少于EKFSLAM和FASTSLAM.

3 實(shí)驗(yàn)

本實(shí)驗(yàn)在稀疏路標(biāo)環(huán)境和密集路標(biāo)環(huán)境下使用CDKFSLAM和FASTSLAM2.0分別進(jìn)行測(cè)試.稀疏路標(biāo)環(huán)境大小為250 m×200 m，包含35個(gè)路標(biāo);密集路標(biāo)環(huán)境大小為250 m×200 m，包含135個(gè)路標(biāo).移動(dòng)機(jī)器人輪間距為4 m，所使用傳感器最大檢測(cè)距離為30 m，檢測(cè)角度范圍為0～180°.SLAM過程中狀態(tài)向量預(yù)測(cè)的頻率為400 Hz，測(cè)量數(shù)據(jù)的獲取頻率為50 Hz.機(jī)器人運(yùn)動(dòng)過程中速度噪聲方差為0.3 m，方位角噪聲方差為3°;傳感器測(cè)量過程中測(cè)量距離噪聲方差為0.1 m，角度方差為1°.CDKFSLAM中狀態(tài)向量中的最大路標(biāo)數(shù)目MAXLM為10，權(quán)重計(jì)算時(shí)所參考的路標(biāo)序列數(shù)目m=10，F(xiàn)ASTSLAM2.0中粒子數(shù)目取PMAX=10.

圖1 CDKFSLAM、EKFSLAM和FASTSLAM內(nèi)存比較

圖2 稀疏路標(biāo)環(huán)境下和密集路標(biāo)環(huán)境下SLAM的比較

在稀疏路標(biāo)環(huán)境下使用 CDKFSLAM和FASTSLAM2.0分別進(jìn)行45次實(shí)驗(yàn).圖3中為每次MMSE(Minimum Mean Square Error)比較.圖4為CDKFSLAM和FASTSLAM2.0在不同時(shí)刻時(shí)狀態(tài)向量預(yù)測(cè)和狀態(tài)向量更新所消耗的時(shí)間.從圖4中可以看出，F(xiàn)ASTSLAM2.0所消耗的時(shí)間要大于CDKFSLAM.

圖3 稀疏路標(biāo)環(huán)境下CDKFSLAM和FASTSLAM的MMSE比較

圖4 稀疏路標(biāo)環(huán)境下CDKFSLAM和FASTSLAM的消耗時(shí)間比較

在密集路標(biāo)環(huán)境下，圖5中為每次MMSE (Minimum Mean Square Error)比較.圖6為CDKFSLAM和FASTSLAM2.0在不同時(shí)刻狀態(tài)向量預(yù)測(cè)和狀態(tài)向量更新所消耗的時(shí)間.從圖6中可以看出FASTSLAM所消耗的時(shí)間要大于CDKFSLAM，與稀疏環(huán)境相比，CDKFSLAM所消耗的時(shí)間增大，這是因?yàn)樵诿芗h(huán)境下動(dòng)態(tài)調(diào)整狀態(tài)向量SV和PV的頻率加大的緣故.

圖5 密集路標(biāo)環(huán)境下CDKFSLAM和FASTSLAM的MMSE比較

4 一致性分析

使用 NEES(NormalizedEstimationError Squared)作為對(duì)移動(dòng)機(jī)器人SLAM過程中定位結(jié)果的一致性分析依據(jù)，NEES定義為

圖6 密集路標(biāo)環(huán)境下CDKFSLAM和FASTSLAM的消耗時(shí)間比較

對(duì)CDKFSLAM和FASTSLAM2.0分別進(jìn)行45次實(shí)驗(yàn)，使用45次的平均NEES作為一致性分析依據(jù).由卡方分布屬性可知符合自由度為45×3的卡方分布，當(dāng)考慮雙邊概率為95%的區(qū)域作為一致性區(qū)域時(shí)可得位于此區(qū)域內(nèi)時(shí)表示當(dāng)前機(jī)器人位置估計(jì)滿足一致性要求，否則表示機(jī)器人位置估計(jì)不滿足一致性要求.圖7(a)為稀疏路標(biāo)環(huán)境下通過45次實(shí)驗(yàn)所得的CDKFSLAM的一致性數(shù)據(jù)，圖7(b)為粒子數(shù)為10時(shí)FASTSLAM2.0的一致性數(shù)據(jù).圖8(a)為密集路標(biāo)環(huán)境下CDKFSLAM的一致性數(shù)據(jù)，圖8(b)為粒子數(shù)為10時(shí)FASTSLAM2.0的一致性數(shù)據(jù).的雙邊區(qū)域?yàn)椋?.32 3.75］，當(dāng)

表1是對(duì)圖7和圖8中位于一致性區(qū)間內(nèi)點(diǎn)數(shù)的統(tǒng)計(jì)，比較可以看出無(wú)論是在稀疏路標(biāo)環(huán)境下還是在密集路標(biāo)環(huán)境下使用CDKFSLAM所得的機(jī)器人位置一致性都要優(yōu)于粒子數(shù)目為10的FASTSLAM2.0.對(duì)比圖3和圖5可以看出對(duì)CDKFSLAM而言在密集路標(biāo)環(huán)境下的MMSE均值要優(yōu)于稀疏路標(biāo)環(huán)境下的結(jié)果，但從一致性結(jié)果(圖7，圖8和表1)上來(lái)看卻恰恰相反，在稀疏環(huán)境下CDKFSLAM的一致性要優(yōu)于密集環(huán)境下的結(jié)果.這是由于在MMSE計(jì)算過程中并沒有考慮協(xié)方差矩陣，由此可以看出在對(duì)SLAM結(jié)果的分析過程中使用一致性分析要比使用MMSE更加合理.

圖7 稀疏路標(biāo)環(huán)境下使用CDKFSLAM和FASTSLAM所得機(jī)器人位置一致性

圖8 密集路標(biāo)環(huán)境下使用CDKFSLAM和FASTSLAM所得機(jī)器人位置一致性

表1 CDKFSLAM和FASTSLAM一致性分析

5 結(jié)論

1)選取合適的參數(shù)時(shí)使用Striling多項(xiàng)式插值方法對(duì)非線性近似可以達(dá)到二級(jí)甚至更高級(jí)的近似，從而提高了SLAM結(jié)果的一致性.

2)使用已經(jīng)測(cè)量到的路標(biāo)統(tǒng)計(jì)信息估計(jì)各個(gè)路標(biāo)對(duì)當(dāng)前狀態(tài)向量的影響權(quán)重，使用各個(gè)路標(biāo)權(quán)重動(dòng)態(tài)調(diào)整狀態(tài)向量和狀態(tài)協(xié)方差將其控制在固定大小內(nèi)雖然在一定程度上丟失了一些歷史信息，但可以使其在密集型路標(biāo)環(huán)境下達(dá)到較快的SLAM速度從而可以提高實(shí)際應(yīng)用的可行性.

3)從實(shí)驗(yàn)結(jié)果可以看出本文算法的SLAM MMSE結(jié)果要優(yōu)于FASTSLAM，從一致性分析中可以看出在稀疏環(huán)境和密集環(huán)境下CDKFSLAM的一致性都優(yōu)于粒子數(shù)目為10的FASTSLAM;另外從一致性結(jié)果和MMSE比較可以看出對(duì)SLAM結(jié)果的評(píng)價(jià)時(shí)使用一致性評(píng)價(jià)要比使用MMSE更加合理.

［1］SMITH R，SELF M，CHESSEMAN P.Estimating uncertain spatial relationships in robots［C］//Autonomous Robot Vehicles.New York，NY:Springer-Verlag New York，Inc，1990:167－193.

［2］DURRANT-WHYTE H，BAILEY T.Simultaneous localization and mapping:Part I［J］.IEEE Robotics and Automation Magazine，2006，13(2):99－110.

［3］BAILEY T，DURRANT-WHYTE H.Simultaneous localization and mapping:Part II［J］.IEEE Robotics and Automation Magazine，2006，13(3):108－117.

［4］KIM C，SAKTHIVEL R，CHUNG W K.Unscented FastSLAM:A robust and efficient solution to the slam problem［J］.IEEE Transactions on Robotics，2008，24(4):808－820.

［5］BAILEY T，NIETO J，NEBOT E.Consistency of the FastSLAM algorithm［C］//Proceeding of IEEE International Conference on Robotics and Automation.Orlando，F(xiàn)L:IEEE，2006:424－429.

［6］MONTEMERLO M，THRUN S，KOLLER D，et al.FastSLAM 2.0:An improved particle filtering algorithm for simultaneous localization and mapping that provably converges［C］//Proceeding of the International Conference on Artificial Intelligence.Acapulco，Mexico: Springer，2003:1151－1156.

［7］THRUN S，KOLLER D，GHAHRAMANI Z，et al.Simultaneous Mapping and Localization with Sparse Extended Information Fulters:Theory and Initial Results［R］.［S.l.］:CMU，2002.

［8］THRUN S，LIU Y F，KOLLER D，et al.Simultaneous mapping and localization with sparse extended information filters［J］.The International Journal of Robotics Research，2004，23(7/8):693－716.

［9］WAN E A，Van der MERWE R.The unscented kalman filter for nonlinear estimation［C］//Proceedings of A-daptive Systems for Signal Processing，Communications and Control Symposium.Lake Louise，Alta:IEEE，2000:153－158.

［10］NORGAARD M，POULSEN N，RAVN O.New developments in state estimation for nonlinear systems［J］.Automatica，2000，36(11):1627－1638.

［11］NORGAARD M，POULSEN N，RAVN O.Advances in Derivative-Free State Estimation for Nonlinear Systems［R］.［S.l.］:Denmark，2004.

［12］Van der MERWE R.Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models［D］.Oregon:Oregon Health＆Science University，2004.

［13］WANG X，ZHANG H.A UPF-UKF framework for SLAM［C］//Proceedings of International Conference on Robotics and Automation.Roma:IEEE，2007:1664－1669.

［14］BAR-SHALOM Y，RONG L X，KIRUBARAJAN T.Estimation with Applications To Tracking and Navigation［M］.New York，NY:Wiley-Interscience Publication，2001.