講解關(guān)于slam一系列文章匯總鏈接:史上最全slam從零開(kāi)始，針對(duì)于本欄目講解的 卡爾曼家族從零解剖 鏈接 :卡爾曼家族從零解剖-(00)目錄最新無(wú)死角講解：https://blog.csdn.net/weixin_43013761/article/details/133846882
?
$文末正下方中心提供了本人聯(lián)系方式，點(diǎn)擊本人照片即可顯示WX\to 官方認(rèn)證$
?

一、前言

上一篇博客，講解的內(nèi)容確實(shí)太多了，推導(dǎo)過(guò)程比較復(fù)雜，為了有一個(gè)整體的認(rèn)知，方便后續(xù)學(xué)習(xí)，所以這里再花費(fèi)一個(gè)篇幅梳理一下知識(shí)點(diǎn)。首先有最基本的貝葉斯公式：
$$f_{X\mid Y}(x\mid y)=\frac{f_{X,Y}(x,y)}{f_Y(y)}=\frac{f_{Y\mid X}(y\mid x)f_X(x)}{{\int }_{?\infty }^{+\infty }{f}_{Y\mid X}(y\mid x)f_X(x)\mathrm{d}x}=\eta f_{Y\mid X}(y\mid x)f_X(x)\text{\hspace{1em}(01)}$$ 有了這個(gè)貝葉斯公式呢，接著做了兩個(gè)重要的假設(shè)：

????????$(01):$ $X_0$ 與 $Q_1$、$Q_2$、$Q_3$、$......$、$Q_k$ 相互獨(dú)立。
????????$(02):$ $X_1$ 與 $R_1$、$R_2$、$R_3$、$......$、$R_k$ 相互獨(dú)立。

基于上上述的假設(shè)，我們假設(shè)了隨機(jī)變量的狀態(tài)方程與觀測(cè)方程：
$$\begin{gathered} X_k=f(X_{k?1})+Q_k \\ {Y}_k=h(X_k)+R_k\text{\hspace{1em}(02)} \end{gathered}$$然后我們進(jìn)行隨機(jī)變量的遞歸過(guò)程，圖示如下(上一篇博客圖二簡(jiǎn)化精華版)：

上圖為大致遞推流程，但是這個(gè)流程是很難走下去的，為什么呢？假設(shè)現(xiàn)在初始狀態(tài) $X_0$、$f(x)$狀態(tài)轉(zhuǎn)移方程、$h(x)$ 測(cè)量方程、 與初始狀態(tài)轉(zhuǎn)移噪聲 $Q_1$ 與 測(cè)量噪聲 $R_1$、以及各個(gè)時(shí)刻的觀測(cè)隨機(jī)變量 $Y_1$到 $Y_k$，但是我們依舊沒(méi)有辦法遞推出 $X_k^{+}$，因?yàn)槲覀儾恢?$Q_2$ 到 $Q_k$，$R_2$ 到 $R_k$，注意，這里是說(shuō)隨機(jī)變量 $Q_k$ 與 $R_k$ 的具體取值不知道是多少，而不是說(shuō)關(guān)于他們的函數(shù)表達(dá)式不知道(后續(xù)學(xué)習(xí)卡爾曼濾波會(huì)有更加深的體會(huì))，所以根據(jù)狀態(tài)方程與測(cè)量方程進(jìn)行了如下推導(dǎo)：

$(01):$ 根據(jù)狀態(tài)方程推導(dǎo)出先驗(yàn)概率密度函數(shù) $f_{X_k}^{?}(x)$，其等價(jià)于(01)式中的 $f_X(x)$。
$$f_{X_k}^{?}(x)=\frac{\mathrm{d}{F}_{X_1}^{?}(x)}{\mathrm{d}x}={\int }_{?\infty }^{+\infty }{f}_{Q_k}[x?f(v)]f_{X_{k?1}}^{+}(v)\mathrm{d}v\text{\hspace{1em}(03)}$$

$(02):$ 根據(jù)測(cè)量方程推導(dǎo)出出似然概率密度函數(shù) $f_{Y_k\mid X_k}(y_k\mid x)$，等價(jià)于(01)式中的 $f_{Y\mid X}(y\mid x)f_X(x)$。$$f_{Y_k\mid X_k}(y_k\mid x)=f_{R_k}\left[y_k?h(x)\right]\text{\hspace{1em}(04)}$$
$(03):$ 最后參照(01)式子，可得后驗(yàn)概率密度函數(shù) $f_{X_k}^{+}(x)$：
$$f_{X_k}^{+}(x)={\eta }_k?f_{X_k|Y_k}(x)?f_{X_k}^{?}(x)={\eta }_k?f_{R_k}\left[y_k?h(x)\right]?{\int }_{?\infty }^{+\infty }{f}_{Q_k}[x?f(v)]f_{X_{k?1}}^{+}(v)\mathrm{d}v\text{\hspace{1em}(05)}$$$${\eta }_k=[{\int }_{?\infty }^{+\infty }(f_{R_k}\left[y_k?h(x)\right]?{\int }_{?\infty }^{+\infty }{f}_{Q_k}[x?f(v)]f_{X_{k?1}}^{+}(v)\mathrm{d}v\mathrm{d}x{]}^{?1})\mathrm{d}x\text{\hspace{1em}(06)}$$
需要注意，其上的 $f_{Q_k}$ 與 $f_{R_k}$ 都為概率密度函數(shù)(PDF)。求得 后驗(yàn)概率密度，對(duì)齊進(jìn)行積分即可得到概率分布，進(jìn)一步求期望即可。

二、技術(shù)難點(diǎn)

公式確實(shí)推導(dǎo)出來(lái)了，但是很明顯上式是一個(gè)廣義結(jié)果，并沒(méi)有實(shí)例化，比如 $f(x)$，$h(x)$。如果這兩個(gè)是函數(shù)十分復(fù)雜，比如非線性。那么上式則設(shè)計(jì)到兩個(gè)非線性無(wú)窮積分的運(yùn)算，也就是 (03) 式與 (06) 式。亦或者 $f_{Q_k}(x)$ 與 $f_{R_k}(x)$ 十分復(fù)雜，同樣會(huì)導(dǎo)致非線性無(wú)窮積分的運(yùn)算，這樣有可能會(huì)導(dǎo)致無(wú)法求解。當(dāng)然，這個(gè)推導(dǎo)出來(lái)的結(jié)果就沒(méi)有任何作用了。

所以呢，在實(shí)際的應(yīng)用中，基于貝葉斯的不通算法會(huì)有不同假設(shè)，比如令 $f(x)$，$h(x)$ 為線性函數(shù)，那么顯然，其就應(yīng)用場(chǎng)景就存在局限性了，卡爾曼濾波就是一種具體化的實(shí)現(xiàn)。另外還有擴(kuò)展卡爾曼濾波，其能夠處理一些非線性的復(fù)雜場(chǎng)景，后續(xù)也會(huì)為大家詳細(xì)分析。依照不通實(shí)現(xiàn)方式，對(duì)基于貝葉斯濾波思想的方法進(jìn)行了歸類如下：

上圖后續(xù)會(huì)繼續(xù)擴(kuò)充，暫時(shí)只做簡(jiǎn)要介紹。需要注意的是，無(wú)論是卡爾曼濾波、例子濾波、直方圖濾波、他們都是屬于部分實(shí)例化，而非完全實(shí)例化。這里回顧一下前面的說(shuō)辭：

$核心(個(gè)人理解)：$ 首先貝葉斯濾波其并不是具體某一種算法的具體實(shí)現(xiàn)，而是一種思想指導(dǎo)，就類似于 C++ 編程中的抽象類，定義了一些虛函數(shù)而已。這里舉一個(gè)恰當(dāng)一些的例子，比如說(shuō)小學(xué)語(yǔ)文課，小朋友會(huì)學(xué)習(xí)筆畫，一橫、一豎、一撇等等。這些就類似于貝葉斯濾波，是比較基礎(chǔ)比較抽象的，但是通過(guò)筆畫的組合，可以實(shí)現(xiàn)各種中文子圖，如你、我、他、好、壞等等，這些實(shí)際的字就好像是卡爾曼濾波、粒子濾波等等。雖然每個(gè)小朋友寫的字，只要寫(筆畫組合)正確了，那么就是一個(gè)正確的字，但是呢？每個(gè)小朋友寫的字跡都是不一樣的，有的好看，有的不好看，有的大，有的小，但是這些字都是屬于小朋友自己的特色。這里就好比，各行各業(yè)，各個(gè)不同的算法，他們或許都會(huì)使用到卡爾曼濾波，但是卻不盡相同，多少會(huì)存在一些區(qū)別。所以我們?cè)谑褂眠@些實(shí)例化的算法，也需要根據(jù)實(shí)際情況做一些改動(dòng)或調(diào)整。

再者，根據(jù)狀態(tài)方程與觀測(cè)方程是否非線性，分為線性(L)系統(tǒng)或非線性(NL)系統(tǒng)，根據(jù)隨機(jī)變量是否符合正太分布分為高斯(G)系統(tǒng)，與非高斯(NG)系統(tǒng)，當(dāng)然他們是可以混合的，比如 LG(線性高斯) 系統(tǒng)就是最簡(jiǎn)單的一類，NLNG(非詳細(xì)非高斯) 系統(tǒng)則是最為復(fù)雜的一類。通常來(lái)說(shuō)最參見(jiàn)的系統(tǒng)是 NLG(非線性高斯) 系統(tǒng)。

三、額外提及

在前面推導(dǎo)過(guò)程中，都是基于一維進(jìn)行推導(dǎo)的，如隨機(jī)變量 $X_k$、$Y_k$、$Q_k$、$R_k$、$?$ 代表的都是一個(gè)數(shù)值，但是我們?cè)趯?shí)際應(yīng)用時(shí)，基本都是二維以上的，如本人在 slam 中遇到的基本都是三維的變量，很少說(shuō)存在一維的，雖然也可以通過(guò)一維的方式解決，但是這樣顯得太 low ，代碼的可讀性會(huì)極度下降。

就拿狀態(tài)變量 $X_k$ 來(lái)說(shuō)，其通常都是 3 維以上的，甚至更多，比如機(jī)器人的狀態(tài)，對(duì)于 slam 來(lái)說(shuō)其至少要包含 旋轉(zhuǎn)與平移，也就是 $R$、$\mathbf{p}$。旋轉(zhuǎn)矩陣 $R$ 可以用多種形式表示，
如使用四元素來(lái)表示旋轉(zhuǎn)，那么共 7=4+3 個(gè)狀態(tài)量，如果使用李代數(shù)來(lái)表示旋轉(zhuǎn)，則共 6=3+3 個(gè)狀態(tài)量?？梢灾赖氖?#xff0c;根據(jù)狀態(tài)方程 $X_k=f(X_{k?1})+Q_k$ 可知，若 X_k 的維度為 n，那么噪聲分布通常維度也為 n，因?yàn)檫@樣才能完成加法運(yùn)算。也比較好理解，噪聲就是狀態(tài)量的抖動(dòng)與不確定性。

根據(jù)前面的推導(dǎo)，可以知道貝葉斯濾波底層原來(lái)是一種遞歸的方式，其本質(zhì)基于前一個(gè)狀態(tài)的概率分布對(duì)下一個(gè)狀態(tài)的概率分布進(jìn)行預(yù)測(cè)，這種方式的計(jì)算量比較少，通常都能夠?qū)崟r(shí)，特別適合 slam 中在線位姿估算。

除了基于貝葉斯濾波這種遞推的方式，還有一種批量?jī)?yōu)化(MAP)的方式，這里先考慮一極端的方式，及考慮到 $0～k$ 時(shí)刻的所有數(shù)據(jù)，同時(shí)對(duì) $0～k$ 時(shí)刻的所有狀態(tài)變量 $x=x_{0:k}=(x_0,?,x_k)$ 進(jìn)行優(yōu)化，首先這里都使用小寫，代表隨機(jī)過(guò)程的具體化，另外這里 $x$ 是一個(gè)宏觀概念，其沒(méi)有下標(biāo)，即代表多個(gè)變量。在數(shù)學(xué)中經(jīng)常使用 $\check{x}$ 表示先驗(yàn)，$\hat{x}$ 表示后驗(yàn)，帽子分別向下與向上(有興趣的朋友可以去閱讀一下機(jī)器人學(xué)中的狀態(tài)估計(jì))。除基本狀態(tài)量外，還有一些額外(控制)量 $v=(\check{x_0},v_{1:k})=(\check{x_0},v_1,?,v_k)$，以及 $0～k$ 時(shí)刻的觀測(cè) $y=y_{0:k}=(y_0,?,y_k)$。那么使用數(shù)學(xué)表達(dá)式，則如下:
$$\hat{x}=\underset{x}{\mathrm{argmax}}p(x|v,y)\text{\hspace{1em}(07)}$$上式表達(dá)的意思是，就是在已知 $v=(\check{x_0},v_{1:k})=(\check{x_0},v_1,?,v_k)$ 與 $y=y_{0:k}=(y_0,?,y_k)$ 的條件下，估算出各個(gè)時(shí)刻狀態(tài)量的后驗(yàn)估計(jì) $\hat{x}=(\widehat{x_0},?,\widehat{x_k})$。注意，這里要求的是 $\hat{x}$ 整體最優(yōu)，而不是某個(gè)時(shí)刻最優(yōu)。根據(jù)聯(lián)合概率與條件概率轉(zhuǎn)換公式，可得如下兩個(gè)等式：$$\begin{gathered} p(x,v,y)=p(x|v,y)?p(y|v)?p(v) \\ p(x,v,y)=p(y|v,x)?p(x|v)?p(v)\text{\hspace{1em}(08)} \end{gathered}$$上面兩式聯(lián)合得：$$p(x|v,y)=\frac{p(y|v,x)?p(x|v)}{p(y|v)}\text{\hspace{1em}(09)}$$這個(gè)形式其實(shí)就是貝葉斯多條件變量形式，故$$\hat{x}=\underset{x}{\mathrm{argmax}}p(x|v,y)=\underset{x}{\mathrm{argmax}}\frac{p(y|v,x)?p(x|v)}{p(y|v)}\text{\hspace{1em}(10)}$$
需要注意的一點(diǎn)式，上式推導(dǎo)過(guò)程都是基于離散形式的，若對(duì)前面知識(shí)點(diǎn)比較深刻，可以猜到離散與連續(xù)還是存在一定區(qū)別的。

對(duì)于批量式的最優(yōu)狀態(tài)估算，并不需要求解后驗(yàn) $\hat{x}$ 的概率分布，只需知道 $p(x|v,y)$ 最大值即可，因?yàn)槠渥畲笾祵?duì)應(yīng)的 $x$ 我們即認(rèn)為是 $\hat{x}$，在進(jìn)一步根據(jù)上式可以知道分母 $p(y|v)$ 與 $x$ 是沒(méi)有關(guān)系的，且我們是求最大值，則可舍去分母：$$\hat{x}=\underset{x}{\mathrm{argmax}}p(x|v,y)=\underset{x}{\mathrm{argmax}}p(y|v,x)?p(x|v)=\underset{x}{\mathrm{argmax}}p(y|x)?p(x|v)\text{\hspace{1em}(11)}$$可以看到上式子最右邊等號(hào)，因?yàn)楦鱾€(gè)時(shí)刻的噪聲是不相關(guān)(高斯分布中等價(jià)于獨(dú)立)，所以 $p(y|x)$ 與 $p(x|v)$ 可以展開(kāi)以乘積的方式表示$$p(x\mid v)=p\left(x_0\mid {\check{x}}_0\right)\prod _{k=1}^{K}p\left(x_k\mid x_{k?1},v_k\right)p(y\mid x)=\prod _{k=0}^{K}p\left(y_k\mid x_k\right)\text{\hspace{1em}(12)}$$其上推導(dǎo)過(guò)程是比較好理解的，也就是把相關(guān)的整合到一起，不相關(guān)的通過(guò)乘積串起來(lái)?？梢钥吹絻蓚€(gè)部分都是=可以分解成累乘的方式，那么很容易想到使用對(duì)數(shù)的形式，變成累加如下：
$$\ln (p(y\mid x)p(x\mid v))=\ln p\left(x_0\mid {\check{x}}_0\right)+\sum _{k=1}^K\ln p\left(x_k\mid x_{k?1},v_k\right)+\sum _{k=0}^K\ln p\left(y_k\mid x_k\right)\text{\hspace{1em}(13)}$$$核心要點(diǎn)：$在前面的博客有提到過(guò)，兩個(gè)高斯分布相乘,得到的結(jié)果仍然為高斯分布，有興趣的朋友可以具體百度或者谷歌一下找到相關(guān)證明。要令上式取得最大值，首先來(lái)分析其第一項(xiàng) $\ln p\left(x_0\mid {\check{x}}_0\right)$，這里要知道的是，需要把其 $x_0$ 看作是變量，因?yàn)?$0～k$ 時(shí)刻的所有狀態(tài)變量 $x=x_{0:k}=(x_0,?,x_k)$ 都是變量。即 $\ln p\left(x_0\mid {\check{x}}_0\right)$ 取最大值，其含義就是說(shuō)基于 ${\check{x}}_0$ 的條件下，$x_0$ 取何值時(shí)，其概率最大。這里的概率 $p$ 是符合高斯分布的，其均值就是 ${\check{x}}_0$，帶入高斯分布公式得：$$\begin{array}{rl}\ln p\left(x_0\mid {\check{x}}_0\right)=& ?\frac{1}{2}{\left(x_0?{\check{x}}_0\right)}^{\mathrm{T}}{\check{P}}_0^{?1}\left(x?{\check{x}}_0\right)?\underset{\text{與?}x\text{?無(wú)關(guān)?}}{\underbrace{\frac{1}{2}\ln \left((2\pi {)}^N\det {\check{P}}_0\right)}}\end{array}\text{\hspace{1em}(14)}$$同理，對(duì)另外的兩項(xiàng)進(jìn)行也同樣帶入到高斯分布得：$$\begin{array}{rl}\ln p\left(x_k\mid x_{k?1},v_k\right)=& ?\frac{1}{2}{\left(x_k?A_{k?1}{x}_{k?1}?v_k\right)}^{\mathrm{T}}{Q}_k^{?1}\left(x_k?A_{k?1}{x}_{k?1}?v_k\right)\\ & ?\underset{\text{與?}x\text{?無(wú)關(guān)?}}{\underbrace{\frac{1}{2}\ln \left((2\pi {)}^N\det Q_k\right)}}\end{array}\text{\hspace{1em}(15)}$$$$\begin{array}{rl}\ln p\left(y_k\mid x_k\right)=& ?\frac{1}{2}{\left(y_k?C_k{x}_k\right)}^{\mathrm{T}}{R}_k^{?1}\left(y_k?C_k{x}_k\right)\\ & ?\underset{\text{與?}x\text{?無(wú)關(guān)?}}{\underbrace{\frac{1}{2}\ln \left((2\pi {)}^M\det R_k\right)}}\end{array}\text{\hspace{1em}(16)}$$上面得(14)(15)(16)式，舍去 $x$ 無(wú)關(guān)項(xiàng)，定義：$$\begin{array}{l}{J}_{v,k}(x)=\{\begin{array}{ll}\frac{1}{2}{\left(x_0?{\check{x}}_0\right)}^{\mathrm{T}}{\tilde{P}}_0^{?1}\left(x_0?{\check{x}}_0\right),& k=0\\ \frac{1}{2}{\left(x_k?A_{k?1}{x}_{k?1}?v_k\right)}^{\mathrm{T}}{Q}_k^{?1}\left(x_k?A_{k?1}{x}_{k?1}?v_k\right),& k=1,?,K\end{array}\\ J_{y,k}(x)=\frac{1}{2}{\left(y_k?C_k{x}_k\right)}^{\mathrm{T}}{R}_k^{?1}\left(y_k?C_k{x}_k\right),k=0,?,K\end{array}$$于是目標(biāo)函數(shù)變?yōu)榍筮@個(gè)式得最小化：$$\hat{x}=\arg \underset{x}{\min }J(x)J(x)=\sum _{k=0}^K\left(J_{v,k}(x)+J_{y,k}(x)\right)\text{\hspace{1em}(17)}$$這個(gè)問(wèn)題就是參見(jiàn)的無(wú)約束最少二乘問(wèn)題，可以看 $J_{v,k}(x)$、$J_{y,k}(x)$ 本質(zhì)上就是狀態(tài)方程與 觀測(cè)方程誤差項(xiàng)總合。

四、結(jié)語(yǔ)

該篇博客，不僅對(duì)貝葉斯濾波理論基礎(chǔ)進(jìn)行了梳理，還額外提及到了離散形式下的批量?jī)?yōu)化。上面舉例的形式是一種極端的形式，即考慮 $0～k$ 時(shí)刻的所有數(shù)據(jù)，可想而知，其應(yīng)用過(guò)程是十分耗時(shí)的。這種方式稱為最大后驗(yàn)估計(jì)(Maximum A Posteriori MAP)，需要注意，其是基于 LG 系統(tǒng)進(jìn)行推斷的。

在實(shí)際應(yīng)用中，也可以只考慮到一個(gè)小段時(shí)間，進(jìn)行優(yōu)化求解，如 $i～k$ 時(shí)刻。另外，還有一些批量?jī)?yōu)化處理與卡爾曼遞推聯(lián)合起來(lái)，一起使用的方法，若后續(xù)時(shí)間充裕，會(huì)為大家分析講解一下。對(duì)貝葉斯濾波有了基本了解之后，下一篇博客我們就分析卡爾曼濾波了。