最近在細讀SEBASTIAN THRUN大神的Probabilistic Robotics。

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

相見恨晚+慚愧

在這里，會不定期更新學習筆記。（文章內(nèi)容圖片基本都來自上述的書）

今天寫一寫泰勒展開式在EKF（Extended KF）的作用。

為什么要用泰勒展開式？

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

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

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

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

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

那么，話說回來，為了處理非線性的問題，從KF 衍生出了EKF。

EKF的基本形態(tài)

基本的KF：

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

z =Cx_t + \delta_t

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

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

那么不線性對使用KF有啥影響？

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

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

KF和EKF的根本問題

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

他就是， 泰勒展開式 （其實泰勒展開式東西我們初中高中好像就學過，一直不知道有啥用）

泰勒公式

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

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

這里利用的是一階泰勒展開式。

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

但需要注意的是，x不是一個特定的值。（個人覺得這個概念很重要，因為一開始的總會把x當做一個值，所以一直不明白 \mu 到底是啥東西）

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

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

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

非線性的函數(shù)g（x）如果沒有做過任何處理的話，會生成下圖中的左上圖中的灰色的概率分布?？赡芩幸欢ǖ囊?guī)律，或者沒有~? 但是能明顯看出來的內(nèi)容就是，他的分布不是正態(tài)分布。

對非線性的g（x）發(fā)動技能：泰勒展開！

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

對x-1進行泰勒展開

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

利用泰勒一級展開就是如下。

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

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

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

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

那么這個時間節(jié)點上， x_t? 就可以利用平均值和方差來表示。

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

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

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

但是x的置信度還是個問題。

所以通過 p(z_t,x_t)? 來驗證x的置信度。(也就是bayesian 理論）

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

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

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

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

測量方程泰勒展開

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

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

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

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

寫了一點關(guān)于泰勒展開式在EKF的作用。

還需解決的問題：

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

EKF線性化的部分因為是近似，但是我們從本文第一張圖的（b）的左上圖可以看出，明顯EKF為了線性化而放棄了精準度。那么有什么更好的方法去擬合非線性的狀態(tài)（測量）方程呢？

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