/*學(xué)習(xí)了@Roger_羅杰的文章，將自己的感悟發(fā)表如下，也可以一起討論。*/
首先下個(gè)結(jié)論，卡爾曼濾波是對有噪音系統(tǒng)的最優(yōu)估計(jì)。
對小車來說，將小車系統(tǒng)看作一正態(tài)分布，我們可以知道當(dāng)前的均值（位姿速度）與方差（誤差），來?[ 預(yù)測 ] 小車下一時(shí)刻的狀態(tài)。即可以根據(jù)T時(shí)刻狀態(tài)和誤差，預(yù)測T+1時(shí)刻位置與方差。（預(yù)測過程是有誤差的，故此時(shí)預(yù)測的T時(shí)刻正態(tài)分布均值變扁了）
預(yù)測完成后，小車運(yùn)行一段時(shí)刻，按照預(yù)測結(jié)果應(yīng)該到達(dá)藍(lán)線這里，但我們現(xiàn)在并不確定是否準(zhǔn)確運(yùn)行至此。為了避免誤差，在T+1時(shí)刻進(jìn)行一次?[ 觀測 ]?，觀測也有誤差（均值與協(xié)方差，觀測值不是真值），記為紅線。
藍(lán)線和紅線一個(gè)是有誤差的預(yù)測值，一個(gè)是不準(zhǔn)的次時(shí)刻觀測值，按常規(guī)思路將之加權(quán)，得到較合理的位置。老頭卡爾曼已經(jīng)證明：加權(quán)是對的，加權(quán)后仍為正態(tài)分布。加權(quán)時(shí)，卡爾曼增益很關(guān)鍵。使用卡爾曼增益修正得到真實(shí)的T+1刻狀態(tài)值和協(xié)方差，記為綠線。 [ 數(shù)據(jù)關(guān)聯(lián) ]
此時(shí)次刻的狀態(tài)和誤差已知，且都是修正過的最優(yōu)值?？梢赃M(jìn)行[狀態(tài)增廣]，如果地圖中已有該狀態(tài)，進(jìn)行狀態(tài)更新，否則進(jìn)行地圖增廣。至此，一次卡爾曼濾波完成，將其視為初態(tài)即可進(jìn)行迭代計(jì)算。
（撒花°*:\(￣▽￣)/:*）
注：數(shù)據(jù)關(guān)聯(lián)是否是得到真值（換句話說，最優(yōu)估計(jì)）尚不得知，是筆者猜測。
