《機(jī)器人智能算法導(dǎo)論》

配套課件本節(jié)簡(jiǎn)介能力目標(biāo)：能運(yùn)用EKF設(shè)計(jì)解決移動(dòng)機(jī)器人定位或SLAM問(wèn)題。能推導(dǎo)狀態(tài)預(yù)測(cè)方程及協(xié)方差矩陣更新公式。能調(diào)試EKF算法中的雅可比矩陣計(jì)算錯(cuò)誤。知識(shí)目標(biāo)：掌握擴(kuò)展卡爾曼濾波器(EKF)的數(shù)學(xué)推導(dǎo)過(guò)程，包括預(yù)測(cè)與校正步驟的核心公式。理解協(xié)方差矩陣的推導(dǎo)邏輯及雅可比矩陣的作用。分析非線性模型線性化對(duì)濾波精度的影響，以及一階泰勒展開的局限性。本講綱要系統(tǒng)模型及觀測(cè)模型的形式非線性模型的求解思路擴(kuò)展卡爾曼濾波器的預(yù)測(cè)步驟擴(kuò)展卡爾曼濾波器的更正步驟向量函數(shù)的導(dǎo)數(shù)擴(kuò)展卡爾曼濾波器的完整算法回顧：線性卡爾曼濾波器在線性卡爾曼濾波器(LKF)中，假設(shè)系統(tǒng)模型是完全線性的。狀態(tài)的演化和觀測(cè)過(guò)程都可以用精確的矩陣運(yùn)算來(lái)描述。狀態(tài)轉(zhuǎn)移模型（線性卡爾曼濾波器）：觀測(cè)模型（線性卡爾曼濾波器）：線性假設(shè)下的高斯傳播圖線性函數(shù)y=ax+b在高斯傳播下的均值變化情況高斯分布線性變換函數(shù)高斯輸出系統(tǒng)模型及觀測(cè)模型的形式從線性卡爾曼濾波器到非線性卡爾曼濾波器:在絕大多數(shù)真實(shí)的機(jī)器人應(yīng)用中，系統(tǒng)模型都是非線性的，例如：機(jī)器人的運(yùn)動(dòng)學(xué)模型通常包含復(fù)雜的三角函數(shù)（如角度變化）激光雷達(dá)或攝像頭等傳感器的觀測(cè)模型可能涉及平方根（如計(jì)算距離)觀測(cè)模型（非線性卡爾曼濾波器）：狀態(tài)轉(zhuǎn)移模型（非線性卡爾曼濾波器）：為t時(shí)刻通過(guò)狀態(tài)變換預(yù)測(cè)的新的狀態(tài)為t?1時(shí)刻經(jīng)過(guò)后驗(yàn)觀察更正后的狀態(tài)為t時(shí)刻的觀察為系統(tǒng)狀態(tài)轉(zhuǎn)移引入的誤差為觀察測(cè)量誤差均為非線性函數(shù)系統(tǒng)模型及觀測(cè)模型的形式當(dāng)一個(gè)服從高斯分布的隨機(jī)變量經(jīng)過(guò)一個(gè)非線性函數(shù)變換后，其輸出的概率分布將不再是標(biāo)準(zhǔn)的高斯分布。濾波過(guò)程將失去其閉合形式的解。高斯分布非線性變換函數(shù)非高斯輸出核心問(wèn)題：高斯分布的傳遞非線性模型的求解思路擴(kuò)展卡爾曼濾波器（EKF）的求解思路：局部線性化近似策略:既然無(wú)法獲得精確的解析解，EKF采用了一種巧妙的近似策略來(lái)解決非線性問(wèn)題。

核心思想:EKF認(rèn)為，雖然整個(gè)非線性函數(shù)是復(fù)雜的，但在某個(gè)足夠小的局部區(qū)域內(nèi)，可以用一條直線來(lái)很好地近似它。這個(gè)“局部區(qū)域”就是當(dāng)前狀態(tài)最可能的估計(jì)點(diǎn)，即高斯分布的均值點(diǎn)。

實(shí)現(xiàn)方法:這種線性化是通過(guò)在當(dāng)前狀態(tài)的均值點(diǎn)對(duì)非線性函數(shù)進(jìn)行一階泰勒級(jí)數(shù)展開來(lái)實(shí)現(xiàn)的。通過(guò)忽略高階項(xiàng)，保留線性部分，從而將一個(gè)復(fù)雜的非線性問(wèn)題轉(zhuǎn)化為一個(gè)在局部近似的線性問(wèn)題。非線性模型的求解思路EKF使用一階泰勒展開近似（1）如圖所示過(guò)程展示了EKF誤差的來(lái)源：即切線與真實(shí)曲線之間的偏離程度。一階泰勒展開近似可視化過(guò)程第一部分當(dāng)非線性程度越強(qiáng)（曲線越彎曲），這種偏離就越大，EKF的近似誤差也就越大。非線性模型的求解思路EKF使用一階泰勒展開近似（2）如圖所示過(guò)程展示了EKF誤差的來(lái)源：即切線與真實(shí)曲線之間的偏離程度。一階泰勒展開近似可視化過(guò)程第二部分當(dāng)非線性程度越強(qiáng)（曲線越彎曲），這種偏離就越大，EKF的近似誤差也就越大。非線性模型的求解思路EKF使用一階泰勒展開近似（3）如圖所示過(guò)程展示了EKF誤差的來(lái)源：即切線與真實(shí)曲線之間的偏離程度。一階泰勒展開近似可視化過(guò)程第三部分當(dāng)非線性程度越強(qiáng)（曲線越彎曲），這種偏離就越大，EKF的近似誤差也就越大。非線性模型的求解思路數(shù)學(xué)表達(dá):將一階泰勒展開應(yīng)用于系統(tǒng)模型，可以得到如下的線性近似方程。

狀態(tài)轉(zhuǎn)移近似(在

點(diǎn)展開):

觀測(cè)模型近似(在

點(diǎn)展開):通過(guò)這種近似，非線性模型在局部被“退化”為了一個(gè)線性模型，從而使我們能夠重新應(yīng)用標(biāo)準(zhǔn)卡爾曼濾波器的推導(dǎo)框架。擴(kuò)展卡爾曼濾波器的預(yù)測(cè)步驟預(yù)測(cè)步驟目標(biāo)輸入:已知t?1時(shí)刻經(jīng)過(guò)觀測(cè)校正后的后驗(yàn)狀態(tài)分布，它服從高斯分布：目標(biāo):基于系統(tǒng)運(yùn)動(dòng)模型，計(jì)算出t時(shí)刻系統(tǒng)狀態(tài)的先驗(yàn)分布。由于采用了線性化近似，這個(gè)分布也被近似為一個(gè)高斯分布：任務(wù):需要推導(dǎo)出預(yù)測(cè)均值

和預(yù)測(cè)協(xié)方差的計(jì)算公式。擴(kuò)展卡爾曼濾波器的預(yù)測(cè)步驟推導(dǎo)：預(yù)測(cè)均值擴(kuò)展卡爾曼濾波器的預(yù)測(cè)步驟推導(dǎo)：預(yù)測(cè)協(xié)方差(1)/(2)因?yàn)殡S機(jī)變量是一個(gè)均值為0、協(xié)方差矩陣為的高斯分布，所以，。由此，我們可以推導(dǎo)出：擴(kuò)展卡爾曼濾波器的預(yù)測(cè)步驟推導(dǎo)：預(yù)測(cè)協(xié)方差(2)/(2)擴(kuò)展卡爾曼濾波器的預(yù)測(cè)步驟預(yù)測(cè)步驟總結(jié)預(yù)測(cè)均值：預(yù)測(cè)方差：

擴(kuò)展卡爾曼濾波器的更正步驟更正步驟目標(biāo)輸入:

任務(wù):推導(dǎo)更新后的均值和協(xié)方差的計(jì)算公式。1.預(yù)測(cè)步驟得到的先驗(yàn)分布：

擴(kuò)展卡爾曼濾波器的更正步驟投影至觀測(cè)空間

預(yù)測(cè)觀測(cè)均值：預(yù)測(cè)觀測(cè)協(xié)方差：預(yù)測(cè)觀測(cè)的協(xié)方差

(也稱為新息協(xié)方差)，由兩部分不確定性構(gòu)成：

:這是由狀態(tài)預(yù)測(cè)的不確定性

經(jīng)過(guò)觀測(cè)函數(shù)映射到觀測(cè)空間后產(chǎn)生的不確定性。

:這是傳感器本身固有的測(cè)量噪聲不確定性。擴(kuò)展卡爾曼濾波器的更正步驟推導(dǎo)：預(yù)測(cè)觀測(cè)均值因?yàn)槭且粋€(gè)均值為0的高斯分布，所以擴(kuò)展卡爾曼濾波器的更正步驟推導(dǎo)：預(yù)測(cè)觀測(cè)協(xié)方差(1)/(2)為了進(jìn)一步簡(jiǎn)化計(jì)算，假設(shè)擴(kuò)展卡爾曼濾波器的更正步驟推導(dǎo)：預(yù)測(cè)觀測(cè)協(xié)方差綜上所述：(2)/(2)擴(kuò)展卡爾曼濾波器的更正步驟

定義:卡爾曼增益Kt?是一個(gè)至關(guān)重要的權(quán)重矩陣，它用于智能地平衡我們對(duì)“模型預(yù)測(cè)”的信任度和對(duì)“傳感器觀測(cè)”的信任度。公式:當(dāng)觀測(cè)噪聲

很大時(shí):

項(xiàng)會(huì)變小，導(dǎo)致整個(gè)

變小。這意味著更新時(shí)更相信預(yù)測(cè)，較少采納觀測(cè)。當(dāng)預(yù)測(cè)不確定性

很大時(shí):

項(xiàng)會(huì)變大，導(dǎo)致整個(gè)

變大。這意味著更新時(shí)更不相信預(yù)測(cè)，更多地采納觀測(cè)來(lái)進(jìn)行修正擴(kuò)展卡爾曼濾波器的更正步驟更新狀態(tài)均值：后驗(yàn)均值(更正后的估計(jì))：先驗(yàn)均值(預(yù)測(cè)的估計(jì))：新息(Innovation)。這是真實(shí)觀測(cè)值與預(yù)測(cè)觀測(cè)值之間的差異。?：卡爾曼增益。它決定了應(yīng)該采納多大比例的“新息”來(lái)修正預(yù)測(cè)值。擴(kuò)展卡爾曼濾波器的更正步驟更新狀態(tài)協(xié)方差式中，協(xié)方差矩陣為對(duì)稱矩陣，即擴(kuò)展卡爾曼濾波器的更正步驟更正步驟總結(jié)第一步：計(jì)算卡爾曼增益(決定權(quán)重)第二步：更新均值(融合觀測(cè)修正估計(jì))第三步：更新協(xié)方差(減小不確定性)向量函數(shù)的導(dǎo)數(shù)雅可比矩陣定義：雅可比矩陣本質(zhì)上是導(dǎo)數(shù)概念從單變量函數(shù)到多變量向量函數(shù)的推廣。它是一個(gè)由所有一階偏導(dǎo)數(shù)構(gòu)成的矩陣作用：對(duì)于一個(gè)非線性函數(shù)，其在某一點(diǎn)的雅可比矩陣定義了該函數(shù)在該點(diǎn)的最佳線性近似。通用形式：對(duì)于函數(shù)，如果是可微的，那么函數(shù)的雅可比矩陣為：向量函數(shù)的導(dǎo)數(shù)

擴(kuò)展卡爾曼濾波器的完整算法討論：EKF的局限性強(qiáng)非線性問(wèn)題：當(dāng)系統(tǒng)或觀測(cè)模型非線性程度非常高時(shí)，一階泰勒展開（用直線近似曲線）會(huì)產(chǎn)生巨大的模型誤差。這可能導(dǎo)致濾波器性能嚴(yán)