最近在細(xì)讀SEBASTIAN THRUN大神的Probabilistic Robotics。

無(wú)知的我現(xiàn)在才知道有這么一本神書(shū)。很多不懂的細(xì)節(jié)都在書(shū)里面有詳細(xì)的說(shuō)明。

相見(jiàn)恨晚+慚愧

在這里，會(huì)不定期更新學(xué)習(xí)筆記。（文章內(nèi)容圖片基本都來(lái)自上述的書(shū)）

今天寫(xiě)一寫(xiě)泰勒展開(kāi)式在EKF（Extended KF）的作用。

為什么要用泰勒展開(kāi)式？

首先，KF 是處理線性問(wèn)題的基于貝葉斯濾波的濾波器。

KF本來(lái)就是為了處理線性高斯系統(tǒng)的濾波器。

用KF的基本條件就是，接受了馬爾科夫假設(shè)和噪聲是滿(mǎn)足高斯分布的假設(shè)。

或者換句話說(shuō)就是，只有接受了貝葉斯濾波的馬爾科夫假設(shè)和噪聲是滿(mǎn)足高斯分布的假設(shè)， KF才會(huì)產(chǎn)生效果。當(dāng)然沒(méi)有這個(gè)假設(shè)，KF會(huì)運(yùn)行，但是沒(méi)有效果罷了。

KF的5大公式中的理念就是，置信度利用均值和方差表示。像粒子濾波器之類(lèi)的，就不需要特別的均值和方差等參數(shù)去表示噪聲的特性，所以也叫非參數(shù)濾波器

那么，話說(shuō)回來(lái)，為了處理非線性的問(wèn)題，從KF 衍生出了EKF。

EKF的基本形態(tài)

基本的KF：

x= Ax_{t-1}+? Bu_t + \varepsilon_t

z =Cx_t + \delta_t

對(duì)比基本的KF，可以看出，EKF把 Ax_{t-1}+? Bu_t \ 的部分用g(u_t,x_t-1) 來(lái)代替， Cx_t 也是一樣用 h(x_t) 代替。 那么這里表現(xiàn)的含義是？

-狀態(tài)轉(zhuǎn)移方程不再是線性的了。

那么不線性對(duì)使用KF有啥影響？

KF對(duì)噪聲的假設(shè)就是，噪聲一定要屬于正態(tài)分布。也就是說(shuō)，對(duì)于一個(gè)狀態(tài)來(lái)說(shuō)，不停的采樣N次的時(shí)候，這個(gè)狀態(tài)的分布應(yīng)該是鐘型曲線。KF的狀態(tài)轉(zhuǎn)移又是線性的。那么正態(tài)分布經(jīng)的狀態(tài)經(jīng)過(guò)線性變化之后，出現(xiàn)的分布圖其實(shí)還是線性的。 如下圖（a）。

但是當(dāng)狀態(tài)轉(zhuǎn)移方程不再是線性了， 那么經(jīng)過(guò)非線性變換的狀態(tài)的分布就不再屬于正態(tài)分布了。如下圖（b）。

KF和EKF的根本問(wèn)題

那么KF的理論的基本假設(shè)（噪聲是正態(tài)分布）就會(huì)崩塌。為了滿(mǎn)足KF的基本假設(shè)，就需要一種方法把非線性的狀態(tài)轉(zhuǎn)移方程轉(zhuǎn)化為線性方程。

他就是， 泰勒展開(kāi)式 （其實(shí)泰勒展開(kāi)式東西我們初中高中好像就學(xué)過(guò)，一直不知道有啥用）

泰勒公式

泰勒公式的作用就是，當(dāng)求一個(gè)非線性的f(x)很困難的時(shí)候，可以利用這個(gè)x的1,2......N階導(dǎo)數(shù)來(lái)近似這個(gè)值。（感覺(jué)也是初高中數(shù)學(xué)。。）

所以利用泰勒公式的特性對(duì)g，h函數(shù)進(jìn)行求導(dǎo)數(shù)，用此方法來(lái)近似實(shí)際的值。注意： 如果g，h的函數(shù)的參數(shù)多，那么為了找出每個(gè)參數(shù)（狀態(tài)）對(duì)函數(shù)的影響，則需要對(duì)每個(gè)參數(shù)（狀態(tài)）進(jìn)行求偏導(dǎo)數(shù)。

這里利用的是一階泰勒展開(kāi)式。

那么泰勒公式里面的x是啥？在機(jī)器人或者無(wú)人車(chē)的定位里面，x就是state。x可以是pose， 可以是position。反正x只是一個(gè)狀態(tài)或者一堆狀態(tài)。

但需要注意的是，x不是一個(gè)特定的值。（個(gè)人覺(jué)得這個(gè)概念很重要，因?yàn)橐婚_(kāi)始的總會(huì)把x當(dāng)做一個(gè)值，所以一直不明白 \mu 到底是啥東西）

x是有特定的概率密度函數(shù)下隨機(jī)生成的值。但是，我們需要參數(shù)去表示p（x）。

所以一般利用平均值 \mu 和方差 \sigma^2 。

那么回到EKF繼續(xù)說(shuō)明泰勒展開(kāi)的作用。

非線性的函數(shù)g（x）如果沒(méi)有做過(guò)任何處理的話，會(huì)生成下圖中的左上圖中的灰色的概率分布。可能他有一定的規(guī)律，或者沒(méi)有~? 但是能明顯看出來(lái)的內(nèi)容就是，他的分布不是正態(tài)分布。

對(duì)非線性的g（x）發(fā)動(dòng)技能：泰勒展開(kāi)！

非線性的g（x）被強(qiáng)制轉(zhuǎn)化為有正態(tài)分布特性的線性方程。泰勒展開(kāi)式的\mu是下圖（右下）p(x）的平均值。

對(duì)x-1進(jìn)行泰勒展開(kāi)

利用導(dǎo)數(shù)求近似的時(shí)候，注意是只對(duì)xt-1求偏導(dǎo)

利用泰勒一級(jí)展開(kāi)就是如下。

根據(jù)狀態(tài)轉(zhuǎn)移方程prediction階段的泰勒展開(kāi)

這里求出的 g(u_t,x_t-1)? 是x的預(yù)測(cè)值，標(biāo)記為 \bar{x} 。

\bar{x}求出來(lái)之后，利用多變量正態(tài)分布概率密度函數(shù)，求x的平均值和方差。

xt和 xt_預(yù)測(cè)值的多變量正態(tài)分布函數(shù)

那么這個(gè)時(shí)間節(jié)點(diǎn)上， x_t? 就可以利用平均值和方差來(lái)表示。

R_t? 是方差。什么的方差？

x和之前求出來(lái)的 \bar x? 的方差。但是 R_t 數(shù)值上應(yīng)該等于什么？據(jù)我以前的知識(shí)，是狀態(tài)方程本身的噪聲的的方差。（但是現(xiàn)在還不確定，以后弄明白了再更新。)

G_t? 是狀態(tài)轉(zhuǎn)移方程的偏導(dǎo)數(shù)。如果state 是vector， 那么 G_t? 就是一個(gè)矩陣。這矩陣叫Jacobian 矩陣，雅克比矩陣。

但是x的置信度還是個(gè)問(wèn)題。

所以通過(guò) p(z_t,x_t)? 來(lái)驗(yàn)證x的置信度。(也就是bayesian 理論）

計(jì)算p(z_t,x_t) 其實(shí)就是計(jì)算z_t，x_t? 這兩個(gè)隨機(jī)變量的概率分布。 也就是說(shuō)，他們可以利用多變量正態(tài)分布概率密度函數(shù)來(lái)求解.

多變量正態(tài)分布概率密度函數(shù)

多變量正態(tài)分布概率密度函數(shù)的別的形態(tài)

（假設(shè)測(cè)量方程也是非線性的）就像對(duì)狀態(tài)轉(zhuǎn)移方程進(jìn)行泰勒展開(kāi)一樣，對(duì)非線性的測(cè)量方程，也要進(jìn)行泰勒一級(jí)展開(kāi)。跟上面狀態(tài)轉(zhuǎn)移方程的展開(kāi)式同一個(gè)意義，故不再解釋。

測(cè)量方程泰勒展開(kāi)

將 z_t, x_t （2個(gè)變量）代入多變量正態(tài)分布概率密度函數(shù)中，可以得到下面的公式。

那么，作為上面這個(gè)公式輸入的的Qt是什么？Ht是什么？

Q_t? 是方差。什么的方差？測(cè)量方程本身的系統(tǒng)方差。

H_t? 是測(cè)量方程的偏導(dǎo)數(shù)。如果state 是vector， 那么 H_t? 就是一個(gè)矩陣。這個(gè)矩陣叫Jacobian 矩陣，雅克比矩陣。

寫(xiě)了一點(diǎn)關(guān)于泰勒展開(kāi)式在EKF的作用。

還需解決的問(wèn)題：

KF系列的濾波器中，協(xié)方差到底是起什么作用的？

EKF線性化的部分因?yàn)槭墙疲俏覀儚谋疚牡谝粡垐D的（b）的左上圖可以看出，明顯EKF為了線性化而放棄了精準(zhǔn)度。那么有什么更好的方法去擬合非線性的狀態(tài)（測(cè)量）方程呢？

繼續(xù)學(xué)習(xí)，下次再更新~