kalman濾波及其在slam中應(yīng)用-wenkub
2023-05-21 18:02:25 本頁面
【正文】 ? 其中， 表示 時刻的狀態(tài)向量， 表示 時刻的輸入向量， 表示 時刻的觀測向量， ? 分別為過程激勵噪聲和觀測噪聲，它們?yōu)橄嗷オ毩?、正態(tài)分布的白噪聲，協(xié)方差分別是 Q， R（這里假設(shè)它們不隨系統(tǒng)狀態(tài)變化而變化） )(kx)()()()1( kwkBukAxkx ????)()()( kvkHxkz ??k )(ku k)(kz k)()( kvkw 、kalman濾波 ? kalman濾波第一步： ? 用系統(tǒng)的過程模型來預(yù)測下一狀態(tài)的系統(tǒng)。 ))|1()1(()|1()1|1( kkHxkzKkkxkkx g ????????gK))|1(()|1( RHkkHPHkkPK TTg ????)|1()1()1( kkHxkzkV ?????Rkalman濾波 ? 為了令 kalman不斷迭代下去，需要更新 K時刻狀態(tài) 的協(xié)方差： ? 時間更新和測量更新不斷重復(fù)迭代，就是kalman最吸引人的特性。 ? 則 kalman增益為： ? K+1時刻的真實估計值為： ? 該時刻更新協(xié)方差為 （即為上邊 3度的計算式） ? )25/(5 ???gK)2325(*)1|1( ?????? kkx)()1|1( ?????? kkP擴展卡爾曼濾波 ? 可以看出，卡爾曼濾波估計是一個線性隨機系統(tǒng)的狀態(tài)。這組測量數(shù)據(jù)由機器人頭部的測距傳感器測得。 SLAM問題 ? K時刻，機器人狀態(tài)可表示為 ? Xv(k)表示機器人的狀態(tài)， Xlm（ k）為路標(biāo)的狀態(tài)，它不隨時間變化而變化 ,可以寫成 Xlm。 0 100 200 300 400 500050100150200250300350400450500m e t r e smetres0 100 200 300 400 500050100150200250300350400
醫(yī)療健康相關(guān)推薦
鄂ICP備17016276號-1