卡爾曼濾波（Kalman Filter）原理理解和測試

Kalman Filter學原理學習

1. Kalman Filter 歷史

Kalman濾波器的歷史，最早要追溯到17世紀，Roger Cotes開始研究最小均方問題。但由於缺少實際案例的支撐（那個時候哪來那麼多雷達啊啥的這些信號啊），Cotes的研究讓人看着顯得很模糊，因此在估計理論的發展中影響很小。17世紀中葉，最小均方估計（Least squares Estimation）理論逐步完善，Tobias Mayer在1750年將其用於月球運動的估計，Leonard Euler在1749年、Pierre Laplace在1787分別用於木星和土星的運動估計。Roger Boscovich在1755用最小均方估計地球的大小。1777年，77歲的Daniel Bernoulli（大名鼎鼎的伯努利）發明了最大似然估計算法。遞歸的最小均方估計理論是由Karl Gauss建立在1809年（好吧，他聲稱在1795年就完成了），當時還有Adrien Legendre在1805年完成了這項工作，Robert Adrain在1808年完成的。
在1880年，丹麥的天文學家Thorvald Nicolai Thiele在之前最小均方估計的基礎上開發了一個遞歸算法，與Kalman濾波非常相似。在某些標量的情況下，Thiele的濾波器與Kalman濾波器時等價的，Thiele提出了估計過程噪聲和測量噪聲中方差的方法（過程噪聲和測量噪聲是Kalman濾波器中關鍵的概念）。上面提到的這麼多研究估計理論的先驅，大多是天文學家而非數學家。現在，大部分的理論貢獻都源自於實際的工程。“There is nothing so practical as a good theory”，應該就是“實踐是檢驗真理的唯一標準”之類吧。
現在，我們的控制論大Wiener終於出場了，還有那個叫Kolmogorov（柯爾莫戈洛夫）的神人。在19世紀40年代，Wiener設計了Wiener濾波器，然而，Wiener濾波器不是在狀態空間進行的（這個學過Wiener濾波的就知道，它是直接從觀測空間z(n)=s(n)+w(n)進行的濾波），Wiener是穩態過程，它假設測量是通過過去無限多個值估計得到的。Wiener濾波器比Kalman濾波器具有更高的自然統計特性。這些也限制其只是更接近理想的模型，要直接用於實際工程中需要足夠的先驗知識（要預知協方差矩陣），美國NASA曾花費多年的時間研究維納理論，但依然沒有在空間導航中看到維納理論的實際應用。
在1950末期，大部分工作開始對維納濾波器中協方差的先驗知識通過狀態空間模型進行描述。通過狀態空間表述後的算法就和今天看到的Kalman濾波已經極其相似了。Johns Hopkins大學首先將這個算法用在了導彈跟蹤中，那時在RAND公司工作的Peter Swerling將它用在了衛星軌道估計，Swerling實際上已經推導出了（1959年發表的）無噪聲系統動力學的Kalman濾波器，在他的應用中，他還考慮了使用非線性系統動力學和和測量方程。可以這樣說，Swerling和發明Kalman濾波器是失之交臂，一線之隔。在kalman濾波器聞名於世之後，他還寫信到AIAA Journal聲討要獲得Kalman濾波器發明的榮譽（然而這時已經給濾波器命名Kalman了）。總結其失之交臂的原因，主要是Swerling沒有直接在論文中提出Kalman濾波器的理論，而只是在實踐中應用。
Rudolph Kalman在1960年發現了離散時間系統的Kalman濾波器，這就是我們在今天各種教材上都能看到的，1961年Kalman和Bucy又推導了連續時間的Kalman濾波器。Ruslan Stratonovich也在1960年也從最大似然估計的角度推導出了Kalman濾波器方程。

---

2. Kalman Filter 白話理解

引自：Highgear
一片綠油油的草地上有一條曲折的小徑,通向一棵大樹.一個要求被提出: 從起點沿着小徑走到樹下。“很簡單。” A說，於是他絲毫不差地沿着小徑走到了樹下。現在，難度被增加了：蒙上眼。“也不難，我當過特種兵。” B說，於是他歪歪扭扭地走到了樹旁。“唉，好久不練，生疏了。” （只憑自己的預測能力）。“看我的，我有 DIY 的 GPS！” C說，於是他像個醉漢似地歪歪扭扭的走到了樹旁。“唉，這個 GPS 沒做好，漂移太大。”（只依靠外界有很大噪聲的測量）“我來試試。” 旁邊一也當過特種兵的拿過 GPS, 蒙上眼，居然沿着小徑很順滑的走到了樹下。（自己能預測+測量結果的反饋）
“這麼厲害！你是什麼人?”
“卡爾曼 ! ”
“卡爾曼？！你就是卡爾曼？”衆人大喫一驚。 “我是說這個 GPS 卡而慢。”

---

引自：Kent Zeng
假設你有兩個傳感器，測的是同一個信號。可是它們每次的讀數都不太一樣，怎麼辦？
取平均。
再假設你知道其中貴的那個傳感器應該準一些，便宜的那個應該差一些。那有比取平均更好的辦法嗎？
加權平均。
怎麼加權？假設兩個傳感器的誤差都符合正態分佈，假設你知道這兩個正態分佈的方差，用這兩個方差值，（此處省略若干數學公式），你可以得到一個“最優”的權重。接下來，重點來了：假設你只有一個傳感器，但是你還有一個數學模型。模型可以幫你算出一個值，但也不是那麼準。怎麼辦？
把模型算出來的值，和傳感器測出的值，（就像兩個傳感器那樣），取加權平均。
OK，最後一點說明：你的模型其實只是一個步長的，也就是說，知道x(k)，我可以求x(k+1)。
問題是x(k)是多少呢？答案：x(k)就是你上一步卡爾曼濾波得到的、所謂加權平均之後的那個對x在k時刻的最佳估計值。
於是迭代也有了。這就是卡爾曼濾波

---

引自：周嘯
假設我養了一隻豬：

一週前，這隻豬的體重是46±0.5kg。注意，在這裏我用了±0.5，表示其實我對這隻豬一週前的體重並不是那麼確定的，也就是說，46kg這個體重有0.5kg的誤差。
現在，我又養了這隻豬一個星期。那麼我想要知道它一個星期之後多重，又大概有多少的誤差？

爲了得到一週後的體重，我有兩種方法：一是根據我多年的養豬經驗得到的豬體重公式推求出一個大概的值，另一個就是直接去稱它的體重。當然，兩種方法都有一定的誤差。假設經驗公式得到的體重是48kg，誤差2kg；直接稱體重得到的是49kg，誤差1kg：

可是，我是一個處女座的人，不管是經驗公式得到的值，還是直接稱量得到的值，我都覺得不夠準。我希望有一種方法，可以同時結合這隻豬一週前的體重、用經驗公式估計的值以及直接稱量得到的值，綜合考慮，得出一個最接近豬真實體重的，誤差最小的值。這就是卡爾曼濾波要完成的任務。現在我們來把養豬的模型抽象成數學公式：

上圖的左邊，上一週的豬的體重，可以抽象爲也k-1時刻的狀態值，用k-1時刻的最優估計值加上一個誤差項來表示，右邊同理。其中，

P=E[ekeTk](31)$$\begin{array}{}\text{(31)}& P=E[e_k{e}_k^T]\end{array}$$

這一項表示的是估計值的協方差。這裏要說明兩點：1，上圖中所有的變量都是用粗體，表示這是一個向量或者一個矩陣；2，之所以用（列）向量而非一個數來表示狀態值，是因爲，雖然一隻豬的體重可以用一個值來表示，但是在實際的應用中很多狀態並不是一個數就能表示的（比如導彈在空間中的位置，同時有x、y、z三個座標）。上圖中右邊表示k時刻的狀態值，這個值可以通過預測模塊（也就是根據經驗公式估計豬的體重）和糾錯模塊（也就是直接去稱量豬的體重值）來估計。同樣，預測模塊和糾錯模塊都有着對應的誤差和誤差協方差矩陣。卡爾曼濾波要做的，就是根據貝葉斯估計的相關理論，同時考慮預測模塊和糾錯模塊的協方差，對誤差小的項賦予較大的權重，對誤差大的項賦予較小的權重，並使預測的誤差最小。

具體的實現過程如下：

---

知乎肖暢的回答也很好，推薦查看，這裏附上幾張簡略圖
在t=0時刻，原始位置

在t=1時刻，預測位置

在t=1時刻，測量位置

結合預測和測量

---

3. 先驗知識

卡爾曼濾波是一種高效率的遞歸濾波器(自迴歸濾波器), 它能夠從一系列的不完全及包含噪聲的測量中，估計動態系統的狀態。卡爾曼濾波的一個典型實例是從一組有限的，包含噪聲的，對物體位置的觀察序列（可能有偏差）預測出物體的位置的座標及速度。

3.1 正太分佈

在概率論中，正態（或高斯）分佈是一個非常普遍的連續概率分佈。先來簡單看一元高斯分佈，其概率密度爲：

p(x)∼N(u,σ2)(21)$$\begin{array}{}\text{(21)}& p(x)\sim N(u,{\sigma }^2)\end{array}$$

f(x|u,σ2)=12πσ2−−−−√e−(x−u)22σ2(22)$$\begin{array}{}\text{(22)}& f(x|u,{\sigma }^2)=\frac{1}{\sqrt{2\pi {\sigma }^2}}{e}^{-\frac{(x-u{)}^2}{2{\sigma }^2}}\end{array}$$

其中，u$u$ 是分佈的均值或期望; σ$\sigma$ 是標準差; σ2${\sigma }^2$ 是方差。下圖爲一元高斯的一個示意圖。

再談多元高斯分佈，k維隨機向量 X=[X1，X2，...，Xk]T$X=[X1，X2，...，Xk{]}^T$ 的多元正態分佈：

X∼N(u,Σ)(23)$$\begin{array}{}\text{(23)}& X\sim N(u,\mathrm{\Sigma })\end{array}$$

這裏舉例二元高斯分佈，其概率密度爲：

正太分佈的性質

1 經過線性變換高斯分佈仍然爲高斯分佈

X∼N(μ,Σ)Y=AX+B}⇒Y∼N(Aμ+B,AΣAT)(5)$$\begin{array}{}\text{(5)}& \begin{array}{l}X\sim N(\mu ,\mathrm{\Sigma })\\ Y=AX+B\end{array}\}\Rightarrow Y\sim N(A\mu +B,A\mathrm{\Sigma }{A}^T)\end{array}$$

2 高斯變量線性組合仍爲高斯分佈

X1∼N(μ1,σ21)X2∼N(μ2,σ22)}⇒p(X1+X2)∼N(μ1+μ2,σ21+σ22+2ρσ1σ2)(6)$$\begin{array}{}\text{(6)}& \begin{array}{c}{X}_1\sim N({\mu }_1,{\sigma }_1^2)\\ X_2\sim N({\mu }_2,{\sigma }_2^2)\end{array}\}\Rightarrow p(X_1+X_2)\sim N\left({\mu }_1+{\mu }_2,{\sigma }_1^2+{\sigma }_2^2+2\rho {\sigma }_1{\sigma }_2\right)\end{array}$$

3 兩個相互獨立的高斯變量的乘積，仍然爲高斯分佈

X1∼N(μ1,σ21)X2∼N(μ2,σ22)}⇒p(X1)⋅p(X2)∼N(σ22σ21+σ22μ1+σ21σ21+σ22μ2,σ21σ22σ21+σ22)(7)$$\begin{array}{}\text{(7)}& \begin{array}{c}{X}_1\sim N({\mu }_1,{\sigma }_1^2)\\ X_2\sim N({\mu }_2,{\sigma }_2^2)\end{array}\}\Rightarrow p(X_1)\cdot p(X_2)\sim N\left(\frac{{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}{\mu }_1+\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}{\mu }_2,\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}\right)\end{array}$$

3.2 中心極限定理

Wiki百科, 在概率論中，中心極限定理（CLT）證明，在大多數情況下，當獨立的隨機變量被加上時，它們正確的歸一化和趨於正態分佈（非正式的“ 鐘形曲線 ”），即使原始變量本身不是正常分佈。該定理是概率論中的一個關鍵概念，因爲它意味着適用於正態分佈的概率統計方法可以適用於涉及其他類型分佈的許多問題。
例如，假設獲得的樣本包含大量的觀測值，每個觀測值都是以不依賴於其他觀測值的方式隨機生成的，並且計算觀測值的算術平均值。如果此過程被執行許多次，中心極限定理說，平均的計算值將被分佈根據正態分佈。一個簡單的例子是，如果一個硬幣多次翻轉在一系列翻轉中獲得給定數量的頭部的概率將接近正態曲線，其平均值等於每個系列中翻轉總數的一半。（在無限次翻轉的限制下，它將等於一條正常曲線。）
中心極限定理有許多變體。在其通常的形式中，隨機變量必須是相同的分佈。

3.3 協方差

Wiki百科, 在概率論和統計學中，協方差是兩個隨機變量的聯合變異性的量度。如果一個變量的較大值主要與另一個變量的較大值相對應，並且對於較小值也是如此，即變量傾向於顯示相似的行爲，則協方差是正的。在相反情況下，當一個變量的值越大，主要對應於其他的，即，更小的值，變量往往顯示相反的行爲，協方差是負的。因此，協方差的符號顯示了線性關係的趨勢之間的變量。協方差的大小不容易解釋，因爲它不是標準化的，因此取決於變量的大小。

定義：具有有限二次矩的兩個聯合分佈的 實值隨機變量 X和Y之間的協方差定義爲它們與其各自期望值的偏差的期望乘積。

cov(X,Y)=E[(X−E[X])][Y−E[Y]](24)$$\begin{array}{}\text{(24)}& cov(X,Y)=E[(X-E[X])][Y-E[Y]]\end{array}$$

其中，E[X]$E[X]$ 是所述預期值的X$X$ ，也被稱爲的平均值X$X$ 。協方差也有時表示σXY$\sigma XY$ 或σ(X,Y)$\sigma (X,Y)$ ，以類似於方差。通過使用期望的線性特性，可以將其簡化爲其產品的期望值減去期望值的乘積.

cov(X,Y)=E[XY]−E[X]E[Y](25)$$\begin{array}{}\text{(25)}& cov(X,Y)=E[XY]-E[X]E[Y]\end{array}$$

3.4 隱含馬爾科夫模型

這裏引用吳軍老師《數學之美》，很不幸我沒找到相關的網頁。大家可以去看書，很好理解。篇幅太長。

這裏簡單說下。先看如下圖所示的典型通信模型示意圖。

其中S1,S2,S3,⋯$S_1,S_2,S_3,\cdots$ 表示信息源發出的信號，比如手機發送的信號。O1,O2,O3,⋯$O_1,O_2,O_3,\cdots$ 是接收器（比如PC或者手機）接收到的信號。通信中的解碼就是根據接收到的信號O1,O2,O3,⋯$O_1,O_2,O_3,\cdots$ 還原爲發送的信號S1,S2,S3,⋯$S_1,S_2,S_3,\cdots$ 。
在通信中，如何如何將O1,O2,O3,⋯$O_1,O_2,O_3,\cdots$ 還原爲發送的信號S1,S2,S3,⋯$S_1,S_2,S_3,\cdots$ ，只需要從所有的源信息中找到最可能產出觀測信號的那一個信號。用概率論的語言來描述，就是在已O1,O2,O3,⋯$O_1,O_2,O_3,\cdots$ 的情況下，求得令條件概率P(S1,S2,S3,⋯|O1,O2,O3,⋯)$P(S_1,S_2,S_3,\cdots |O_1,O_2,O_3,\cdots )$ 達到最大值的那個信息串S1,S2,S3,⋯$S_1,S_2,S_3,\cdots$ ，即：

S1,S2,S3,⋯=ArgMaxP(S1,S2,S3,⋯|O1,O2,O3,⋯)(29)$$\begin{array}{}\text{(29)}& S_1,S_2,S_3,\cdots =ArgMaxP(S_1,S_2,S_3,\cdots |O_1,O_2,O_3,\cdots )\end{array}$$

其中Arg是參數Argument的縮寫，表示能獲得最大值的那個信息串S1,S2,S3,⋯$S_1,S_2,S_3,\cdots$ 。
可是怎麼求呢？利用貝葉斯公式間接求：

P(O1,O2,O3,⋯|S1,S2,S3,⋯)⋅P(S1,S2,S3,⋯)P(O1,O2,O3,⋯)(30)$$\begin{array}{}\text{(30)}& \frac{P(O_1,O_2,O_3,\cdots |S_1,S_2,S_3,\cdots )\cdot P(S_1,S_2,S_3,\cdots )}{P(O_1,O_2,O_3,\cdots )}\end{array}$$

其中，P(O1,O2,O3,⋯|S1,S2,S3,⋯)$P(O_1,O_2,O_3,\cdots |S_1,S_2,S_3,\cdots )$ 表示信息S1,S2,S3,⋯$S_1,S_2,S_3,\cdots$ 在傳輸後變成O1,O2,O3,⋯$O_1,O_2,O_3,\cdots$ 的可能性；而P(S1,S2,S3,⋯)$P(S_1,S_2,S_3,\cdots )$ 表示本身是一個合乎情理的句子，同樣P(O1,O2,O3,⋯)$P(O_1,O_2,O_3,\cdots )$ 表示說話的人產生的信息O1,O2,O3,⋯$O_1,O_2,O_3,\cdots$ 的合理性。
再來考慮如何簡化，一旦信息產生了他就不會改變了，這時P(O1,O2,O3,⋯)$P(O_1,O_2,O_3,\cdots )$ 就是一個可以忽略的常數了。然後剩下的項就可以使用Hidden Markov Model來估計了。
圖略
兩個假設：

     1. 馬爾科夫假設：任意時刻的狀態只依賴於其前一個時刻的狀態，即si$s_i$ 的狀態只依賴於si−1$s_{i-1}$ 。
     2. 獨立輸出假設：任意時刻的觀測只依賴於當前時刻的裝填，即oi$o_i$ 的狀態只依賴於si$s_i$ 。

  在繼續之前，我們需要了解隱馬爾可夫鏈的狀態轉移以及觀測序列的生成過程，假設隱藏狀態空間（未知的輸入信號狀態）包含N$N$ 個狀態(s1,s2,s3,⋯,sN)$(s_1,s_2,s_3,\cdots ,s_N)$ ，此時隱馬爾可夫鏈可以形象描述爲一個多段圖G$G$ （學過算法的都知道），每一層包含N$N$ 個節點，每個節點表示狀態空間中的一個狀態si$s_i$ ，相鄰層之間的節點全連接，那麼從第一層到最後一層（假設包含T$T$ 層）的路徑也就有NT−1條，每一條路徑（記爲P$P$ ）都會包含T個節點，每一條路徑都會以P(O1,O2,O3,⋯|S1,S2,S3,⋯)$P(O_1,O_2,O_3,\cdots |S_1,S_2,S_3,\cdots )$ 生成一個觀測序列。
  我們此時已知變量有：多段圖G$G$ 的初始狀態；節點之間的轉移概率；觀測序列中節點的生成概率；我們的問題是如何根據這個結構以及觀測序列來得到一個最優路徑（概率最大路徑）
我們可以使用維特比算法來得到最優路徑，實質上是一個簡單的動態規劃問題。

---

4. Kalman Filter 詳述

4.1 模型

卡爾曼濾波模型理論建立在線性代數和隱含馬爾可夫模型。其基本動態系統可以用一個馬爾可夫鏈表示，該馬爾可夫鏈建立在一個被高斯噪聲(即正態分佈的噪聲)干擾的線性算子上的。系統的狀態可以用一個元素爲實數的向量表示。 隨着離散時間的每一個增加,這個線性算子就會作用在當前狀態上，產生一個新的狀態,並也會帶入一些噪聲,同時系統的一些已知的控制器的控制信息也會被加入。同時，另一個受噪聲干擾的線性算子產生出這些隱含狀態的可見輸出。
模型圖如下圖所示：

4.2 模型假設

• 系統的狀態方程是線性的；
• 觀測方程是線性的；
• 過程噪聲符合零均值高斯分佈；
• 觀測噪聲符合零均值高斯分佈；

滿足以上4點假設，一直在線性變化的空間中操作高斯分佈，狀態的概率密度符合高斯分佈。卡爾曼濾波是一種遞歸的估計，只要獲知上一時刻狀態的估計值以及當前狀態的觀測值就可以計算出當前狀態的估計值，因此不需要記錄觀測或者估計的歷史信息。

卡爾曼濾波器的遞歸過程：

1) 估計時刻k$k$ 的狀態：

X(k)=A∗X(k−1)+B∗u(k)(13)$$\begin{array}{}\text{(13)}& X(k)=A\ast X(k-1)+B\ast u(k)\end{array}$$

u(k)$u(k)$ ，是系統輸入，l$l$ 維向量，表示k$k$ 時刻的輸入;
X(k)$X(k)$ ，n$n$ 維向量，表示k$k$ 時刻觀測狀態的均值;
A$A$ ，n∗n$n\ast n$ 矩陣，表示狀態從k−1$k-1$ 到k$k$ 在沒有輸入影響時轉移方式;
B$B$ ，n∗n$n\ast n$ 矩陣，表示u(k)$u(k)$ 如何影響x(k)$x(k)$ .

2) 計算誤差相關矩陣P$P$ , 度量估計值的精確程度：

P(k)=A∗P(k−1)∗AT+Q(14)$$\begin{array}{}\text{(14)}& P(k)=A\ast P(k-1)\ast A^T+Q\end{array}$$

Pk$P_k$ ，n∗n$n\ast n$ 方差矩陣，表示k$k$ 時刻被觀測的n$n$ 個狀態的方差。
Q=E(W2j)$Q=E(W_j^2)$ 是系統噪聲的協方差陣，即系統框圖中的Wj$W_j$ 的協方差陣, Q$Q$ 應該是不斷變化的，爲了簡化，當作一個常數矩陣。

3) 計算卡爾曼增益：

K(k)=P(k)∗HT∗(H∗P(k)∗HT+R)−1(15)$$\begin{array}{}\text{(15)}& K(k)=P(k)\ast H^T\ast (H\ast P(k)\ast H^T+R{)}^{-1}\end{array}$$

這裏R=E(V2j)$R=E(V_j^2)$ , 是測量噪聲的協方差(陣), 即系統框圖中的 Vj$V_j$ 的協方差, 爲了簡化，也當作一個常數矩陣;
H$H$ ，m∗n$m\ast n$ 矩陣，表示狀態x(k)$x(k)$ 如何被轉換爲觀測z(k)$z(k)$ 。
由於我們的系統一般是單輸入單輸出，所以R$R$ 是一個1×1$1\times 1$ 的矩陣，即一個常數，上面的公式可以簡化爲:

K=P(k)∗HT(H∗P(k)∗HT+R)(16)$$\begin{array}{}\text{(16)}& K=\frac{P(k)\ast H^T}{(H\ast P(k)\ast H^T+R)}\end{array}$$

4) 狀態變量反饋的誤差量：

e=Z(k)–H∗X(k)(17)$$\begin{array}{}\text{(17)}& e=Z(k)–H\ast X(k)\end{array}$$

這裏的 Z(k)$Z(k)$ 是帶噪聲的測量量

5) 更新誤差相關矩陣P$P$

P(k)=P(k)–K∗H∗P(k)(18)$$\begin{array}{}\text{(18)}& P(k)=P(k)–K\ast H\ast P(k)\end{array}$$

6) 更新狀態變量：

X(k)=X(k)+K∗e=X(k)+K∗(Z(k)–H∗X(k))(19)$$\begin{array}{}\text{(19)}& X(k)=X(k)+K\ast e=X(k)+K\ast (Z(k)–H\ast X(k))\end{array}$$

7) 最後的輸出：

Y(k)=H∗X(k)(20)$$\begin{array}{}\text{(20)}& Y(k)=H\ast X(k)\end{array}$$

---

5. Kalman Filter 測試

---

6. 總結

---

REFERENCE

1 http://bbs.21ic.com/icview-292853-1-1.html
2 http://blog.csdn.net/xiahouzuoxin/article/details/39582483
3 http://blog.csdn.net/yangtrees/article/details/8075911
4 http://www.cnblogs.com/ycwang16/p/5999034.html
5 http://blog.csdn.net/heyijia0327/article/details/17487467
6 http://blog.csdn.net/heyijia0327/article/details/17667341
7 https://en.wikipedia.org/wiki/Kalman_filter
Paper:http://www.cl.cam.ac.uk/~rmf25/papers/Understanding%20the%20Basis%20of%20the%20Kalman%20Filter.pdf