簡介
轉自：http://www.cnblogs.com/ymxiansen/p/5368547.html
已經歷經了半個世紀的卡爾曼濾波至今仍然是研究的熱點，相關的文章不斷被髮表。其中許多文章是關於卡爾曼濾波器的新應用，但也不乏改善和擴展濾波器算法的研究。而對算法的研究多着重於將卡爾曼濾波應用於非線性系統。
爲什麼學界要這麼熱衷於將卡爾曼濾波器用於非線性系統呢？因爲卡爾曼濾波器從一開始就是爲線性系統設計的算法，不能用於非線性系統中。但是事實上多數系統都是非線性的，所以如果卡爾曼濾波器不能用在非線性系統中的話，那麼它的應用範圍就非常有限了。如果真的是這樣，卡爾曼濾波器可能早就壽終正寢或者過很久很久纔會被人注意到。幸運的是早期的學者們對這個問題理解的非常深刻，而且也找到了解決方法，就是擴展卡爾曼濾波（EKF）。
事實上世界上的第一個卡爾曼濾波也是擴展卡爾曼濾波，而不是線性卡爾曼濾波器。擴展卡爾曼濾波有很久遠的歷史，如果說有一個非線性系統需要用到卡爾曼濾波的話，不必懷疑，先試試擴展卡爾曼濾波準沒錯。因爲他有很久遠的歷史，所以可以輕鬆的找到許多這方面的資料。
不過擴展卡爾曼濾波也不是無懈可擊的，它有一個很嚴重的短板——發散。使用擴展卡爾曼濾波的時候請務必記在心上，時刻提醒自己，這樣設計濾波器其結果會發散嗎？毫不誇張地說相對於線性卡爾曼濾波設計擴展卡爾曼濾波器的就是在解決發散問題。發散問題解決了剩下的都是小事。
小結：
- 擴展卡爾曼濾波器主要用於非線性系統；
- 擴展卡爾曼濾波器會發散。
線性化的卡爾曼濾波器
在討論擴展卡爾曼濾波之前，首先要了解一下線性化卡爾曼濾波。它和線性卡爾曼濾波器在濾波器的算法方面有同樣的算法結構，一樣一樣的。不一樣的地方在於這兩者的系統模型不同。線性卡爾曼濾波器的系統本身就是線性系統，而線性化卡爾曼濾波器的系統本身是非線性系統，但是機智的大神們將非線性的系統進行了線性化，於是卡爾曼濾波就可以用在非線性系統中了。對於一個卡爾曼濾波器的設計者，就不要去管你的模型到底是一開始就是線性系統還是非線性系統線性化得到的線性系統，反正只要是線性系統就好了。好了，現在你有了一個線性系統，那你還需要擔心什麼呢？這就是一個之前講過的線性卡爾曼濾波器啦。
的確是這樣的，沒有很大的差別，但是請跟我一起念：線性化卡爾曼濾波器會發散。爲什麼會發散呢？是這樣，我們在對非線性系統進行線性化的過程中，只有被線性化的那個點附近的線性化模型和真實的模型相近，遠的誤差就大了，那麼這個時候卡爾曼濾波器的效果就不好。如果懂點線性化知識這個道理就很明顯。所以線性化的這個限制要時刻考慮，這也就是爲什麼要把線性卡爾曼濾波器和線性化卡爾曼濾波器區分開的理由。
而決定一個線性化濾波器成功與否的關鍵就在於這個濾波器系統模型線性化得好不好。一個貼近於真實模型的線性化模型對於濾波器的良好輸出非常重要。所以說掌握如何線性化一個非線性模型很重要，然而我們並不會討論關於系統線性化的問題，因爲這已經不屬於卡爾曼濾波的範疇了，而且每個系統有不同的線性化方法，有點複雜。
總而言之，如果你已經明白瞭如何使用線性卡爾曼濾波器，那麼使用線性化卡爾曼濾波器就沒有什麼需要擔心的，因爲算法結構一樣嘛。唯一需要注意的就是線性化卡爾曼濾波器會發散，要在有效範圍內使用，要不然濾波器的表現不會好。
小結：
- 線性化卡爾曼濾波器與線性卡爾曼濾波器有同樣的算法結構，不同樣的系統模型；
- 線性化卡爾曼濾波器會發散，原因在於函數在遠於被線性化的點的時候並不接近於非線性函數；
- 線性化卡爾曼的表現取決於線性化做得好不好。
擴展卡爾曼濾波器
在這節具體介紹卡爾曼濾波器。首先從比較熟悉的線性卡爾曼濾波器開始比較擴展卡爾曼濾波器與線性化卡爾曼濾波器的異同，從系統模型到濾波器算法，並解釋這些不同。之後將提供兩個具體的應用例子來加以體會。這門書的重點在於如何感性的理解和使用卡爾曼濾波器，所以對於算法的推導不會被具體描述。但是如何理解和區別這些不同，這些不同表達了什麼意義將會一一解釋。如下圖所示，右面是我們已經熟悉的經典的線性化卡爾曼濾波的算法結構和步驟，左面則是擴展卡爾曼濾波器的算法結構和步驟，其中不同的地方已經用紅筆圈出來。可以看到兩個濾波器的算法結構是相同的，只有幾個方程上有細微的差別。
非線性系統模型
在非線性系統中，系統模型是這樣的：
xk+1 ＝ f（Xk）＋ wk
zk ＝ h（Xk） ＋ vk
這與線性系統的區別在於非線性系統的狀態向量和其係數是不能夠分離的。
比如說，在GPS定位的僞距與接收機位置的關係中：ρ［Xu］＝ ［（xu－xsi）2 + （yu －ysi）2 ＋（zu － zsi）2 ］1/2 ＋ b ＋ vi ，
其中［Xu］＝［xu，yu,zu,b］是系統狀態向量，分別是接收機的位置和接收機與衛星的鐘差，
［xsi，ysi，zsi］是衛星的位置座標，ρ是僞距，vi是觀測噪聲。
在這個關係中狀態向量與它的係數就是不可分離的，沒有辦法寫成AXk的形式，只能是f（Xk）。h（Xk）的存在同理。
擴展卡爾曼濾波器算法
比較上圖，可以看到差別主要在這樣兩個地方：
- 第 I 步中xk+1 ＝ f（xk－1），與原來的xk+1 ＝ Axk－1；
- 第 III 步中Xk ＝ Xk－ ＋ Kk（zk － h（xk－）），與原來的Xk ＝ Xk－ ＋ Kk（zk － Hxk－）；
其他的地方則完全一樣。那麼是不是只需要改動這兩點就可以將一個線性卡爾曼濾波變成擴展卡爾曼濾波呢？不是的，有一些更重要的差別隱藏在公式中。在右圖中，也就是線性卡爾曼濾波器中矩陣A和矩陣H是已知的，在而左圖中雖然將第I步和第III步中的A和H替換成f和h，但是其他地方的A和H卻仍然存在，可是在非線性系統中，哪有A和H呢？
A和H的獲得就要涉及到之前所提到的決定擴展卡爾曼濾波器表現的決定因素：線性化。線性化的方法很經典：
將非線性系統中的f對x求（xk估計）處的偏導得到A，同樣的求h對x求（xk－）處的偏導得到H。（向量和矩陣怎麼求偏導？）
線性化濾波器和擴展卡爾曼濾波器的共同點在於他們都需要經歷一個線性化的過程，不同點在於，擴展卡爾曼濾波器是將xk估計作爲線性化的參考點，線性化卡爾曼濾波器不是。（線性化濾波器是用什麼作爲線性化參考點？參考點是不是就是求偏導以後的帶入值？） 在設計擴展卡爾曼濾波器的時候是不是知道這一點並不會有什麼不同。但是如果你在猶豫我是要用擴展卡爾曼濾波器還是用線性化卡爾曼濾波器的時候，明白這一點是非常重要的。下面對線性化濾波器和卡爾曼濾波器線性化參考點的差異做簡單的解釋。
在擴展卡爾曼濾波中，我們並不用前一個時刻的先驗值Xk－（卡爾曼濾波器未經過修正的預測值）作爲參考點，而是用前一個時刻的估計值作爲參考點做線性化。這是因爲相對於先驗值，前一個時刻的估計值更加貼近於真實值，將估計值作爲線性化參考點可以得到一個更加貼近於實際的線性化系統模型。這種線性化方法跟適合難以提前確定線性化參考點的系統模型。而相反的。如果說線性化參考點已經確定了，那麼完全不必用前一刻的估計值作爲線性化參考點。比如說在對衛星的位置這樣的系統模型進行線性化的時候，由於衛星的運動軌跡有一個連續的軌道，在這種情況下，就不必用前一個時刻的估計值作爲線性化參考點。（而是直接用系統對下一個時刻的預測就可以了？）
總而言之，你只要有個概念，擴展卡爾曼濾波器是基於先驗估計做系統線性化的就可以了。具體的細節在實驗中就會有所體會。重要的是我們知道了A和H是根據上面兩個公式得到的。
總結一下這一小結講的什麼。我們看到總體而言擴展卡爾曼濾波器的結構過程都和線性卡爾曼濾波器相同。但是每一步的等式都有一些細微的差別，這些差別可以分爲兩塊：第一個是擴展卡爾曼濾波器用非線性系統系統方程f和h替換了線性系統的A和H。第二個是擴展卡爾曼濾波器中的矩陣A和矩陣H是非線性系統的雅可比行列式（什麼鬼）。除了這兩塊剩下的都和線性卡爾曼濾波器相同。
主要問題如下：
1. 運動及觀察模型用泰勒級數的一階或二階展開近似成線性模型，忽略了高階項，不可避免的引入線性誤差，甚至導致濾波器發散。有如下誤差補償方法：
泰勒近似使得狀態預測必然存在誤差：
A) 補償狀態預測中的誤差，附加“人爲過程噪聲”，即通過增大過程噪聲協方差來實現這一點。
B) 擴大狀態預測協方差矩陣，用標量加權因子φ>1乘狀態預測協方差矩陣
C) 利用對角矩陣φ=diag(sqrt(φi)), φi>1來乘以狀態預測協方差矩陣
其實無論增大過程噪聲協方差還是狀態預測協方差矩陣，都是爲了增大kalman增益，即狀態預測是不準的，我要減小一步狀態預測在狀態更新中的權重。
2. 雅克比矩陣（一階）及海塞矩陣（二階）計算困難。二階EKF的性能要好於一階的，而二階以上的性能相比於二階並沒有太大的提高，所以超過二階以上的EKF一般不採用。但二階EKF的性能雖好，但計算量大，一般情況下不用
小結：
- 在第I步和第III步中，用f和h代替原來的A和H；
- 剩下的A和H，分別求f和h在（xk估計）和（xk－）處的偏導得到；
- 擴展卡爾曼濾波器用前一個時刻的估計值作爲參考點做線性化。
總結：
所謂擴展卡爾曼濾波器，就是適用於非線性系統的卡爾曼濾波器。它與經典的線性卡爾曼濾波器很相似，算法步驟和結構都相同。不同在於系統模型和矩陣A和H。在擴展卡爾曼濾波器當中用非線性系統模型方程代替線性系統墨香的系統方程；將系統模型求偏導得到新的擴展卡爾曼濾波器當中的矩陣A和H，在偏導的求解過程中，也是就是線性化的過程中用前一個時刻的估計值作爲參考點。通過這樣的修改就得到了適用於非線性系統的擴展卡爾曼濾波器。在使用的過程中我們要時刻牢記，擴展卡爾曼濾波會發散。下一篇是實驗。
未解決的問題：1.如何對非線性系統線性化，求偏導就可以嗎？2.矩陣和向量怎麼求偏導？3.爲什麼f和h求偏導的點不同？4.線性化參考點是不是就是求偏導以後的參考點？5.雅可比行列式？