Kalman濾波器從原理到實現(xiàn)

轉載請注明出處：http://xiahouzuoxin.github.io/notes

Kalman濾波器的歷史淵源

We are like dwarfs on the shoulders of giants, by whose grace we see farther than they. Our study of the works of the ancients enables us to give fresh life to their finer ideas, and rescue them from time’s oblivion and man’s neglect.

―― Peter of Blois, late twelfth century

太喜歡第一句話了，“我們是巨人肩膀上的矮人，巨人們的優(yōu)雅讓我么看得更比他們更遠”，誰說不是呢？

說起Kalman濾波器的歷史，最早要追溯到17世紀，Roger Cotes開始研究最小均方問題。但由于缺少實際案例的支撐（那個時候哪來那么多雷達啊啥的這些信號啊），Cotes的研究讓人看著顯得很模糊，因此在估計理論的發(fā)展中影響很小。17世紀中葉，最小均方估計（Least squares Estimation）理論逐步完善，Tobias Mayer在1750年將其用于月球運動的估計，Leonard Euler在1749年、Pierre Laplace在1787分別用于木星和土星的運動估計。Roger Boscovich在1755用最小均方估計地球的大小。1777年，77歲的Daniel Bernoulli（大名鼎鼎的伯努利）發(fā)明了最大似然估計算法。遞歸的最小均方估計理論是由Karl Gauss建立在1809年（好吧，他聲稱在1795年就完成了），當時還有Adrien Legendre在1805年完成了這項工作，Robert Adrain在1808年完成的，至于到底誰是Boss，矮子們就別管了吧！

在1880年，丹麥的天文學家Thorvald Nicolai Thiele在之前最小均方估計的基礎上開發(fā)了一個遞歸算法，與Kalman濾波非常相似。在某些標量的情況下，Thiele的濾波器與Kalman濾波器時等價的，Thiele提出了估計過程噪聲和測量噪聲中方差的方法（過程噪聲和測量噪聲是Kalman濾波器中關鍵的概念）。

上面提到的這么多研究估計理論的先驅，大多是天文學家而非數(shù)學家。現(xiàn)在，大部分的理論貢獻都源自于實際的工程。“There is nothing so practical as a good theory”，應該就是“實踐是檢驗真理的唯一標準”之類吧。

現(xiàn)在，我們的控制論大Wiener終于出場了，還有那個叫Kolmogorov（柯爾莫戈洛夫）的神人。在19世紀40年代，Wiener設計了Wiener濾波器，然而，Wiener濾波器不是在狀態(tài)空間進行的（這個學過Wiener濾波的就知道，它是直接從觀測空間z(n)=s(n)+w(n)進行的濾波），Wiener是穩(wěn)態(tài)過程，它假設測量是通過過去無限多個值估計得到的。Wiener濾波器比Kalman濾波器具有更高的自然統(tǒng)計特性。這些也限制其只是更接近理想的模型，要直接用于實際工程中需要足夠的先驗知識（要預知協(xié)方差矩陣），美國NASA曾花費多年的時間研究維納理論，但依然沒有在空間導航中看到維納理論的實際應用。

在1950末期，大部分工作開始對維納濾波器中協(xié)方差的先驗知識通過狀態(tài)空間模型進行描述。通過狀態(tài)空間表述后的算法就和今天看到的Kalman濾波已經極其相似了。Johns Hopkins大學首先將這個算法用在了導彈跟蹤中，那時在RAND公司工作的Peter Swerling將它用在了衛(wèi)星軌道估計，Swerling實際上已經推導出了（1959年發(fā)表的）無噪聲系統(tǒng)動力學的Kalman濾波器，在他的應用中，他還考慮了使用非線性系統(tǒng)動力學和和測量方程。可以這樣說，Swerling和發(fā)明Kalman濾波器是失之交臂，一線之隔。在kalman濾波器聞名于世之后，他還寫信到AIAA Journal聲討要獲得Kalman濾波器發(fā)明的榮譽（然而這時已經給濾波器命名Kalman了）。總結其失之交臂的原因，主要是Swerling沒有直接在論文中提出Kalman濾波器的理論，而只是在實踐中應用。

Rudolph Kalman在1960年發(fā)現(xiàn)了離散時間系統(tǒng)的Kalman濾波器，這就是我們在今天各種教材上都能看到的，1961年Kalman和Bucy又推導了連續(xù)時間的Kalman濾波器。Ruslan Stratonovich也在1960年也從最大似然估計的角度推導出了Kalman濾波器方程。

目前，卡爾曼濾波已經有很多不同的實現(xiàn)。卡爾曼最初提出的形式現(xiàn)在一般稱為簡單卡爾曼濾波器。除此以外，還有施密特擴展濾波器、信息濾波器以及很多Bierman, Thornton開發(fā)的平方根濾波器的變種。也許最常見的卡爾曼濾波器是鎖相環(huán)，它在收音機、計算機和幾乎任何視頻或通訊設備中廣泛存在。

從牛頓到卡爾曼

從現(xiàn)在開始，就要進行Kalman濾波器探討之旅了，我們先回到高一，從物理中小車的勻加速直線運動開始。

話說，有一輛質量為m的小車，受恒定的力F，沿著r方向做勻加速直線運動。已知小車在t-ΔT時刻的位移是s(t-1)，此時的速度為v(t-1)。求：t時刻的位移是s(t)，速度為v(t)？

由牛頓第二定律，求得加速度：

那么就有下面的位移和速度關系：

如果將上面的表達式用矩陣寫在一起，就變成下面這樣：

卡爾曼濾波器是建立在動態(tài)過程之上，由于物理量（位移，速度）的不可突變特性，這樣就可以通過t-1時刻估計（預測）t時刻的狀態(tài)，其狀態(tài)空間模型為：

對比一下(1)(2)式，長得及其相似有木有：

勻加速直線運動過程就是卡爾曼濾波中狀態(tài)空間模型的一個典型應用。下面我們重點關注(2)式，鑒于研究的計算機信號都是離散的，將(2)是表示成離散形式為：

其中各個量之間的含義是：

1. x(n)是狀態(tài)向量，包含了觀測的目標（如：位移、速度）
2. u(n)是驅動輸入向量，如上面的運動過程是通過受力驅動產生加速度，所以u(n)和受力有關
3. A是狀態(tài)轉移矩陣，其隱含指示了“n-1時刻的狀態(tài)會影響到n時刻的狀態(tài)（這似乎和馬爾可夫過程有些類似）”

4. B是控制輸入矩陣，其隱含指示了“n時刻給的驅動如何影響n時刻的狀態(tài)”

   從運動的角度，很容易理解：小車當前n時刻的位移和速度一部分來自于n-1時刻的慣性作用，這通過Ax(n)來度量，另一部分來自于現(xiàn)在n時刻小車新增加的外部受力，通過Bu(n)來度量。

5. w(n)是過程噪聲，w(n)~N(0,Q)的高斯分布，過程噪聲是使用卡爾曼濾波器時一個重要的量，后面會進行分析。

計算n時刻的位移，還有一種方法：拿一把長的卷尺（嗯，如果小車跑了很長時間，估計這把卷尺就難買到了），從起點一拉，直接就出來了，設測量值為z(n)。計算速度呢？速度傳感器往那一用就出來了。

然而，初中物理就告訴我們，“尺子是量不準的，物體的物理真實值無法獲得”，測量存在誤差，我們暫且將這個誤差記為v(n)。這種通過直接測量的方式獲得所需物理量的值構成觀測空間：

z(n)就是測量結果，H(n)是觀測矢量，x(n)就是要求的物理量（位移、速度），v(n)~N(0,R)為測量噪聲，同狀態(tài)空間方程中的過程噪聲一樣，這也是一個后面要討論的量。大部分情況下，如果物理量能直接通過傳感器測量，。

現(xiàn)在就有了兩種方法（如上圖）可以得到n時刻的位移和速度：一種就是通過(3)式的狀態(tài)空間遞推計算（Prediction），另一種就是通過(4)式直接拿尺子和傳感器測量（Measurement）。致命的是沒一個是精確無誤的，就像上圖看到的一樣，分別都存在0均值高斯分布的誤差w(n)和v(n)。

那么，我最終的結果是取尺子量出來的好呢，還是根據(jù)我們偉大的牛頓第二定律推導出來的好呢？抑或兩者都不是！

一場遞推的游戲

為充分利用測量值（Measurement）和預測值（Prediction），Kalman濾波并不是簡單的取其中一個作為輸出，也不是求平均。

設預測過程噪聲w(n)~N(0,Q)，測量噪聲v(n)~N(0,R)。Kalman計算輸出分為預測過程和修正過程如下：

1. 預測

   預測值：

   

   最小均方誤差矩陣：

   

2. 修正

   誤差增益：

   

   修正值：

   

   最小均方誤差矩陣：

   

從(5)~(9)中：

• x(n)：Nx1的狀態(tài)矢量
• z(n)：Mx1的觀測矢量，Kalman濾波器的輸入
• x(n|n-1)：用n時刻以前的數(shù)據(jù)進行對n時刻的估計結果
• x(n|n)：用n時刻及n時刻以前的數(shù)據(jù)對n時刻的估計結果，這也是Kalman濾波器的輸出

• P(n|n-1)：NxN，最小預測均方誤差矩陣，其定義式為

  

  通過計算最終得到(6)式。

• P(n|n)：NxN，修正后最小均方誤差矩陣。
• K(n)：NxM，誤差增益，從增益的表達式看，相當于“預測最小均方誤差”除以“n時刻的測量誤差+預測最小均方誤差”，直觀含義就是用n-1預測n時刻狀態(tài)的預測最小均方誤差在n時刻的總誤差中的比重，比重越大，說明真值接近預測值的概率越大，這也可以從(8)式中看到。

Kalman濾波算法的步驟是(5)(6)->(7)->(8)(9)。當然，建議找本教材復習下上面公式的推導過程，或參見Wiki上的介紹http://en.wikipedia.org/wiki/Kalman_filter。

公式就是那么的抽象，一旦認真研究懂了卻也是茅塞頓開，受益也比只知皮毛的多。盡管如此，我還算更喜歡先感性后理性。仍以上面的運動的例子來直觀分析：

Example:

還可以更簡單一些：設小車做勻速（而非勻加速）直線運動，方便計算，假設速度絕對的恒定（不波動，所以相關的方差都為0），則u(t)==0恒成立。設預測（過程）位移噪聲w(n)~N(0,2^2)，測量位移噪聲v(n)~N(0,1^2)，n-1狀態(tài)的位移，速度為v=10m/s，n時刻與n-1時刻的物理時差為ΔT=1s。同時，也用尺子測了一下，結果位移為z(n)=62m。

則A = [1 ΔT; 0 1]=[1 1; 0 1]，根據(jù)(5)，預測值為

。

現(xiàn)在已經有了估計值和測量值，哪個更接近真值，這就通過最小均方誤差矩陣來決定！

要求已知上次的修正后的最小均方誤差P(n-1|n-1)=[1 0; 0 0]（勻速，所以P(2,2)=0，右斜對角線上為協(xié)方差，值為0，P(1,1)為n-1時刻位移量的均方誤差，因為要計算P(1,1)還得先遞推往前計算P(n-2|n-2)，所以這里暫時假設為1），則根據(jù)(6)式，最小預測預測均方誤差為P(n|n-1)=[1 0; 0 0][1 1; 0 1][1 0; 0 0]=[1 0; 0 0]。

由物理量的關系知，H(n)=[1 1]，增益K(n)=[1;0]{1+[1 1][1 0; 0 0][1; 1]}^(-1)=[1/2;0]。

所以，最后的n時刻估計值既不是用n-1得到的估計值，也不是測量值，而是：，因此，最終的Kalman濾波器的輸出位移是60.5m。

從上面的遞推關系知道，要估計n時刻就必須知道n-1時刻，那么n=0時刻該如何估計，因此，卡爾曼濾波要初始化的估計值x(-1|-1)和誤差矩陣P(-1|-1)，設x(-1,-1)~N(Us, Cs)，則初始化：

綜上，借用一張圖說明一下Kalman濾波算法的流程：

圖中的符號和本文符號稍有差異，主要是P的表示上。從上圖也可以看出，Kalman濾波就是給定-1時刻的初始值，然后在預測（狀態(tài)空間）和修正（觀測空間）之間不停的遞推，求取n時刻的估計x和均方誤差矩陣P。

均方誤差中的門道

到這里，應該對Kalman濾波有個總體的概念了，有幾個觀點很重要，是建立Kalman濾波器的基礎：

1. 一個是n-1對n時刻估計值，一個是n時刻的測量值，估計值和測量值都存在誤差，且誤差都假設滿足獨立的高斯分布
2. Kalman濾波器就是充分結合了估計值和測量值得到n時刻更接近真值的估計結果
3. Kalman濾波器引入狀態(tài)空間的目的是避免了“像Wiener濾波器一樣需要對過去所有[0,n-1]時刻協(xié)方差先驗知識都已知”，而直接可以通過上一時刻即n-1時刻的狀態(tài)信息和均方誤差信息就可遞推得到n時刻的估計。盡管遞推使得實際應用中方便了，但n-1對n時刻的估計實際上使用到了所有前[0,n-1]時刻的信息，只不過信息一直通過最小均方誤差進行傳遞到n-1時刻。基于此，Kalman濾波也需要先驗知識，即-1時刻的初始值。

在上小節(jié)中只看到Kalman的結論，那么Kalman濾波器是如何將估計值和測量值結合起來，如何將信息傳遞下去的呢？這其中，“獨立高斯分布”的假設條件功勞不可謂不大！測量值z(n)~N(uz,σz^2)，估計值x(n)~N(ux,σx^2)。

Kalman濾波器巧妙的用“獨立高斯分布的乘積”將這兩個測量值和估計值進行融合！

如下圖：估計量的高斯分布和測量量的高斯分布經過融合后為綠色的高斯分布曲線。

稍微計算一下，通過上式求出u和σ^2，

現(xiàn)在令

則(10)(11)變成：

到這里，請將(13)-(14)與(8)-(9)式對比！標量的情況下，在小車的應用中有：A=1，H=1，正態(tài)分布的均值u就是我們要的輸出結果，正態(tài)分布的方差σz^2就是最小均方誤差。推廣到矢量的情況，最小均方誤差矩陣就是多維正態(tài)分布的協(xié)方差矩陣。

從(12)式也很容易看到卡爾曼增益K的含義：就是估計量的方差占總方差（包括估計方差和測量方差）的比重。

一切都變得晴朗起來了，然而這一切的一切，卻都源自于“估計量和測量量的獨立高斯分布”這條假設。進一步總結Kalman濾波器：

假設狀態(tài)空間的n-1時刻估計值和觀測空間的n時刻測量值都滿足獨立高斯分布，Kalman濾波器就是通過高斯分布的乘積運算將估計值和測量值結合，獲得最接近真值的n時刻估計。

高斯分布乘積運算的結果仍為高斯分布，高斯分布的均值對應n時刻的估計值，高斯分布的方差對應n時刻的均方誤差。

Matlab程序看過來

下面的一段Matlab代碼是從網(wǎng)上找到的，程序簡單直接，但作為學習分析用很棒，

% KALMANF - updates a system state vector estimate based upon an % observation, using a discrete Kalman filter. % % Version 1.0, June 30, 2004 % % This tutorial function was written by Michael C. Kleder % % INTRODUCTION % % Many people have heard of Kalman filtering, but regard the topic % as mysterious. While it's true that deriving the Kalman filter and % proving mathematically that it is "optimal" under a variety of % circumstances can be rather intense, applying the filter to % a basic linear system is actually very easy. This Matlab file is % intended to demonstrate that. % % An excellent paper on Kalman filtering at the introductory level, % without detailing the mathematical underpinnings, is: % "An Introduction to the Kalman Filter" % Greg Welch and Gary Bishop, University of North Carolina % http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html % % PURPOSE: % % The purpose of each iteration of a Kalman filter is to update % the estimate of the state vector of a system (and 生活不易，碼農辛苦
如果您覺得本網(wǎng)站對您的學習有所幫助,可以手機掃描二維碼進行捐贈