簡(jiǎn)介

　　已經(jīng)歷經(jīng)了半個(gè)世紀(jì)的卡爾曼濾波至今仍然是研究的熱點(diǎn)，相關(guān)的文章不斷被發(fā)表。其中許多文章是關(guān)于卡爾曼濾波器的新應(yīng)用，但也不乏改善和擴(kuò)展濾波器算法的研究。而對(duì)算法的研究多著重于將卡爾曼濾波應(yīng)用于非線性系統(tǒng)。

　　為什么學(xué)界要這么熱衷于將卡爾曼濾波器用于非線性系統(tǒng)呢？因?yàn)榭柭鼮V波器從一開(kāi)始就是為線性系統(tǒng)設(shè)計(jì)的算法，不能用于非線性系統(tǒng)中。但是事實(shí)上多數(shù)系統(tǒng)都是非線性的，所以如果卡爾曼濾波器不能用在非線性系統(tǒng)中的話，那么它的應(yīng)用范圍就非常有限了。如果真的是這樣，卡爾曼濾波器可能早就壽終正寢或者過(guò)很久很久才會(huì)被人注意到。幸運(yùn)的是早期的學(xué)者們對(duì)這個(gè)問(wèn)題理解的非常深刻，而且也找到了解決方法，就是擴(kuò)展卡爾曼濾波（EKF）。

　　事實(shí)上世界上的第一個(gè)卡爾曼濾波也是擴(kuò)展卡爾曼濾波，而不是線性卡爾曼濾波器。擴(kuò)展卡爾曼濾波有很久遠(yuǎn)的歷史，如果說(shuō)有一個(gè)非線性系統(tǒng)需要用到卡爾曼濾波的話，不必懷疑，先試試擴(kuò)展卡爾曼濾波準(zhǔn)沒(méi)錯(cuò)。因?yàn)樗泻芫眠h(yuǎn)的歷史，所以可以輕松的找到許多這方面的資料。

　　不過(guò)擴(kuò)展卡爾曼濾波也不是無(wú)懈可擊的，它有一個(gè)很嚴(yán)重的短板——發(fā)散。使用擴(kuò)展卡爾曼濾波的時(shí)候請(qǐng)務(wù)必記在心上，時(shí)刻提醒自己，這樣設(shè)計(jì)濾波器其結(jié)果會(huì)發(fā)散嗎？毫不夸張地說(shuō)相對(duì)于線性卡爾曼濾波設(shè)計(jì)擴(kuò)展卡爾曼濾波器的就是在解決發(fā)散問(wèn)題。發(fā)散問(wèn)題解決了剩下的都是小事。

小結(jié)：

• 擴(kuò)展卡爾曼濾波器主要用于非線性系統(tǒng)；
• 擴(kuò)展卡爾曼濾波器會(huì)發(fā)散。

線性化的卡爾曼濾波器

　　在討論擴(kuò)展卡爾曼濾波之前，首先要了解一下線性化卡爾曼濾波。它和線性卡爾曼濾波器在濾波器的算法方面有同樣的算法結(jié)構(gòu)，一樣一樣的。不一樣的地方在于這兩者的系統(tǒng)模型不同。線性卡爾曼濾波器的系統(tǒng)本身就是線性系統(tǒng)，而線性化卡爾曼濾波器的系統(tǒng)本身是非線性系統(tǒng)，但是機(jī)智的大神們將非線性的系統(tǒng)進(jìn)行了線性化，于是卡爾曼濾波就可以用在非線性系統(tǒng)中了。對(duì)于一個(gè)卡爾曼濾波器的設(shè)計(jì)者，就不要去管你的模型到底是一開(kāi)始就是線性系統(tǒng)還是非線性系統(tǒng)線性化得到的線性系統(tǒng)，反正只要是線性系統(tǒng)就好了。好了，現(xiàn)在你有了一個(gè)線性系統(tǒng)，那你還需要擔(dān)心什么呢？這就是一個(gè)之前講過(guò)的線性卡爾曼濾波器啦。

　　的確是這樣的，沒(méi)有很大的差別，但是請(qǐng)跟我一起念：線性化卡爾曼濾波器會(huì)發(fā)散。為什么會(huì)發(fā)散呢？是這樣，我們?cè)趯?duì)非線性系統(tǒng)進(jìn)行線性化的過(guò)程中，只有被線性化的那個(gè)點(diǎn)附近的線性化模型和真實(shí)的模型相近，遠(yuǎn)的誤差就大了，那么這個(gè)時(shí)候卡爾曼濾波器的效果就不好。如果懂點(diǎn)線性化知識(shí)這個(gè)道理就很明顯。所以線性化的這個(gè)限制要時(shí)刻考慮，這也就是為什么要把線性卡爾曼濾波器和線性化卡爾曼濾波器區(qū)分開(kāi)的理由。

　　而決定一個(gè)線性化濾波器成功與否的關(guān)鍵就在于這個(gè)濾波器系統(tǒng)模型線性化得好不好。一個(gè)貼近于真實(shí)模型的線性化模型對(duì)于濾波器的良好輸出非常重要。所以說(shuō)掌握如何線性化一個(gè)非線性模型很重要，然而我們并不會(huì)討論關(guān)于系統(tǒng)線性化的問(wèn)題，因?yàn)檫@已經(jīng)不屬于卡爾曼濾波的范疇了，而且每個(gè)系統(tǒng)有不同的線性化方法，有點(diǎn)復(fù)雜。

　　總而言之，如果你已經(jīng)明白了如何使用線性卡爾曼濾波器，那么使用線性化卡爾曼濾波器就沒(méi)有什么需要擔(dān)心的，因?yàn)樗惴ńY(jié)構(gòu)一樣嘛。唯一需要注意的就是線性化卡爾曼濾波器會(huì)發(fā)散，要在有效范圍內(nèi)使用，要不然濾波器的表現(xiàn)不會(huì)好。

小結(jié)：

• 線性化卡爾曼濾波器與線性卡爾曼濾波器有同樣的算法結(jié)構(gòu)，不同樣的系統(tǒng)模型；
• 線性化卡爾曼濾波器會(huì)發(fā)散，原因在于函數(shù)在遠(yuǎn)于被線性化的點(diǎn)的時(shí)候并不接近于非線性函數(shù)；
• 線性化卡爾曼的表現(xiàn)取決于線性化做得好不好。

擴(kuò)展卡爾曼濾波器

　　 在這節(jié)具體介紹卡爾曼濾波器。首先從比較熟悉的線性卡爾曼濾波器開(kāi)始比較擴(kuò)展卡爾曼濾波器與線性化卡爾曼濾波器的異同，從系統(tǒng)模型到濾波器算法，并解釋這些不同。之后將提供兩個(gè)具體的應(yīng)用例子來(lái)加以體會(huì)。這門(mén)書(shū)的重點(diǎn)在于如何感性的理解和使用卡爾曼濾波器，所以對(duì)于算法的推導(dǎo)不會(huì)被具體描述。但是如何理解和區(qū)別這些不同，這些不同表達(dá)了什么意義將會(huì)一一解釋。如下圖所示，右面是我們已經(jīng)熟悉的經(jīng)典的線性化卡爾曼濾波的算法結(jié)構(gòu)和步驟，左面則是擴(kuò)展卡爾曼濾波器的算法結(jié)構(gòu)和步驟，其中不同的地方已經(jīng)用紅筆圈出來(lái)。可以看到兩個(gè)濾波器的算法結(jié)構(gòu)是相同的，只有幾個(gè)方程上有細(xì)微的差別。

　　非線性系統(tǒng)模型

　　在非線性系統(tǒng)中，系統(tǒng)模型是這樣的：

x_{k 1} ＝ f（X_k）＋ w_k

z_k ＝ h（X_k） ＋ v_k

　　這與線性系統(tǒng)的區(qū)別在于非線性系統(tǒng)的狀態(tài)向量和其系數(shù)是不能夠分離的。

　　比如說(shuō)，在GPS定位的偽距與接收機(jī)位置的關(guān)系中：ρ［X_u］＝ ［（x_u－x_si）²  （y_u －y_si）² ＋（z_u － z_si）² ］^1/2 ＋ b ＋ v_i，

　　其中［X_u］＝［x_u，y_u,z_u,b］是系統(tǒng)狀態(tài)向量，分別是接收機(jī)的位置和接收機(jī)與衛(wèi)星的鐘差，

　　　　［x_si，y_si，z_si］是衛(wèi)星的位置坐標(biāo)，ρ是偽距，vi是觀測(cè)噪聲。

　　在這個(gè)關(guān)系中狀態(tài)向量與它的系數(shù)就是不可分離的，沒(méi)有辦法寫(xiě)成AX_k的形式，只能是f（X_k）。h（X_k）的存在同理。

　　擴(kuò)展卡爾曼濾波器算法

　　比較上圖，可以看到差別主要在這樣兩個(gè)地方：

1. 第  I   步中x_{k 1} ＝ f（x_k－1），與原來(lái)的x_{k 1} ＝ Ax_k－1；
2. 第 III 步中X_k ＝ X_k^－ ＋ K_k（z_k － h（x_k^－）），與原來(lái)的X_k ＝ X_k^－ ＋ K_k（z_k － Hx_k^－）；

　　其他的地方則完全一樣。那么是不是只需要改動(dòng)這兩點(diǎn)就可以將一個(gè)線性卡爾曼濾波變成擴(kuò)展卡爾曼濾波呢？不是的，有一些更重要的差別隱藏在公式中。在右圖中，也就是線性卡爾曼濾波器中矩陣A和矩陣H是已知的，在而左圖中雖然將第I步和第III步中的A和H替換成f和h，但是其他地方的A和H卻仍然存在，可是在非線性系統(tǒng)中，哪有A和H呢？

　　A和H的獲得就要涉及到之前所提到的決定擴(kuò)展卡爾曼濾波器表現(xiàn)的決定因素：線性化。線性化的方法很經(jīng)典：

　　將非線性系統(tǒng)中的f對(duì)x求（x_k估計(jì)）處的偏導(dǎo)得到A，同樣的求h對(duì)x求（x_k^－）處的偏導(dǎo)得到H。（向量和矩陣怎么求偏導(dǎo)？）

　　線性化濾波器和擴(kuò)展卡爾曼濾波器的共同點(diǎn)在于他們都需要經(jīng)歷一個(gè)線性化的過(guò)程，不同點(diǎn)在于，擴(kuò)展卡爾曼濾波器是將xk估計(jì)作為線性化的參考點(diǎn)，線性化卡爾曼濾波器不是。（線性化濾波器是用什么作為線性化參考點(diǎn)？參考點(diǎn)是不是就是求偏導(dǎo)以后的帶入值？）　在設(shè)計(jì)擴(kuò)展卡爾曼濾波器的時(shí)候是不是知道這一點(diǎn)并不會(huì)有什么不同。但是如果你在猶豫我是要用擴(kuò)展卡爾曼濾波器還是用線性化卡爾曼濾波器的時(shí)候，明白這一點(diǎn)是非常重要的。下面對(duì)線性化濾波器和卡爾曼濾波器線性化參考點(diǎn)的差異做簡(jiǎn)單的解釋。

　　在擴(kuò)展卡爾曼濾波中，我們并不用前一個(gè)時(shí)刻的先驗(yàn)值Xk－（卡爾曼濾波器未經(jīng)過(guò)修正的預(yù)測(cè)值）作為參考點(diǎn)，而是用前一個(gè)時(shí)刻的估計(jì)值作為參考點(diǎn)做線性化。這是因?yàn)橄鄬?duì)于先驗(yàn)值，前一個(gè)時(shí)刻的估計(jì)值更加貼近于真實(shí)值，將估計(jì)值作為線性化參考點(diǎn)可以得到一個(gè)更加貼近于實(shí)際的線性化系統(tǒng)模型。這種線性化方法跟適合難以提前確定線性化參考點(diǎn)的系統(tǒng)模型。而相反的。如果說(shuō)線性化參考點(diǎn)已經(jīng)確定了，那么完全不必用前一刻的估計(jì)值作為線性化參考點(diǎn)。比如說(shuō)在對(duì)衛(wèi)星的位置這樣的系統(tǒng)模型進(jìn)行線性化的時(shí)候，由于衛(wèi)星的運(yùn)動(dòng)軌跡有一個(gè)連續(xù)的軌道，在這種情況下，就不必用前一個(gè)時(shí)刻的估計(jì)值作為線性化參考點(diǎn)。（而是直接用系統(tǒng)對(duì)下一個(gè)時(shí)刻的預(yù)測(cè)就可以了？）

　　總而言之，你只要有個(gè)概念，擴(kuò)展卡爾曼濾波器是基于先驗(yàn)估計(jì)做系統(tǒng)線性化的就可以了。具體的細(xì)節(jié)在實(shí)驗(yàn)中就會(huì)有所體會(huì)。重要的是我們知道了A和H是根據(jù)上面兩個(gè)公式得到的。

　　總結(jié)一下這一小結(jié)講的什么。我們看到總體而言擴(kuò)展卡爾曼濾波器的結(jié)構(gòu)過(guò)程都和線性卡爾曼濾波器相同。但是每一步的等式都有一些細(xì)微的差別，這些差別可以分為兩塊：第一個(gè)是擴(kuò)展卡爾曼濾波器用非線性系統(tǒng)系統(tǒng)方程f和h替換了線性系統(tǒng)的A和H。第二個(gè)是擴(kuò)展卡爾曼濾波器中的矩陣A和矩陣H是非線性系統(tǒng)的雅可比行列式（什么鬼）。除了這兩塊剩下的都和線性卡爾曼濾波器相同。

小結(jié)：

• 在第I步和第III步中，用f和h代替原來(lái)的A和H；
• 剩下的A和H，分別求f和h在（x_k估計(jì)）和（x_k^－）處的偏導(dǎo)得到；
• 擴(kuò)展卡爾曼濾波器用前一個(gè)時(shí)刻的估計(jì)值作為參考點(diǎn)做線性化。

 總結(jié)：

　　所謂擴(kuò)展卡爾曼濾波器，就是適用于非線性系統(tǒng)的卡爾曼濾波器。它與經(jīng)典的線性卡爾曼濾波器很相似，算法步驟和結(jié)構(gòu)都相同。不同在于系統(tǒng)模型和矩陣A和H。在擴(kuò)展卡爾曼濾波器當(dāng)中用非線性系統(tǒng)模型方程代替線性系統(tǒng)墨香的系統(tǒng)方程；將系統(tǒng)模型求偏導(dǎo)得到新的擴(kuò)展卡爾曼濾波器當(dāng)中的矩陣A和H，在偏導(dǎo)的求解過(guò)程中，也是就是線性化的過(guò)程中用前一個(gè)時(shí)刻的估計(jì)值作為參考點(diǎn)。通過(guò)這樣的修改就得到了適用于非線性系統(tǒng)的擴(kuò)展卡爾曼濾波器。在使用的過(guò)程中我們要時(shí)刻牢記，擴(kuò)展卡爾曼濾波會(huì)發(fā)散。下一篇是實(shí)驗(yàn)。

　　未解決的問(wèn)題：1.如何對(duì)非線性系統(tǒng)線性化，求偏導(dǎo)就可以嗎？2.矩陣和向量怎么求偏導(dǎo)？3.為什么f和h求偏導(dǎo)的點(diǎn)不同？4.線性化參考點(diǎn)是不是就是求偏導(dǎo)以后的參考點(diǎn)？5.雅可比行列式？