轉(zhuǎ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)雅讓我么看得更比他們更遠(yuǎn)”，誰說不是呢？

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

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

上面提到的這么多研究估計(jì)理論的先驅(qū)，大多是天文學(xué)家而非數(shù)學(xué)家?，F(xiàn)在，大部分的理論貢獻(xiàn)都源自于實(shí)際的工程?！癟here is nothing so practical as a good theory”，應(yīng)該就是“實(shí)踐是檢驗(yàn)真理的唯一標(biāo)準(zhǔn)”之類吧。

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

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

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

目前，卡爾曼濾波已經(jīng)有很多不同的實(shí)現(xiàn)?？柭畛跆岢龅男问浆F(xiàn)在一般稱為簡單卡爾曼濾波器。除此以外，還有施密特?cái)U(kuò)展濾波器、信息濾波器以及很多Bierman, Thornton開發(fā)的平方根濾波器的變種。也許最常見的卡爾曼濾波器是鎖相環(huán)，它在收音機(jī)、計(jì)算機(jī)和幾乎任何視頻或通訊設(shè)備中廣泛存在。

從牛頓到卡爾曼

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

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

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

那么就有下面的位移和速度關(guān)系：

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

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

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

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

其中各個(gè)量之間的含義是：

x(n)是狀態(tài)向量，包含了觀測的目標(biāo)（如：位移、速度）

u(n)是驅(qū)動(dòng)輸入向量，如上面的運(yùn)動(dòng)過程是通過受力驅(qū)動(dòng)產(chǎn)生加速度，所以u(n)和受力有關(guān)

A是狀態(tài)轉(zhuǎn)移矩陣，其隱含指示了“n-1時(shí)刻的狀態(tài)會影響到n時(shí)刻的狀態(tài)（這似乎和馬爾可夫過程有些類似）”

B是控制輸入矩陣，其隱含指示了“n時(shí)刻給的驅(qū)動(dòng)如何影響n時(shí)刻的狀態(tài)”

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

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

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

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

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

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

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

一場遞推的游戲

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

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

預(yù)測

預(yù)測值：

最小均方誤差矩陣：

修正

誤差增益：

修正值：

?這里的A去掉，參見下面的圖

最小均方誤差矩陣：

從(5)~(9)中：

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

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

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

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

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

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

Example:

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

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

。

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

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

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

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

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

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

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

均方誤差中的門道

到這里，應(yīng)該對Kalman濾波有個(gè)總體的概念了，有幾個(gè)觀點(diǎn)很重要，是建立Kalman濾波器的基礎(chǔ)：

一個(gè)是n-1對n時(shí)刻估計(jì)值，一個(gè)是n時(shí)刻的測量值，估計(jì)值和測量值都存在誤差，且誤差都假設(shè)滿足獨(dú)立的高斯分布

Kalman濾波器就是充分結(jié)合了估計(jì)值和測量值得到n時(shí)刻更接近真值的估計(jì)結(jié)果

Kalman濾波器引入狀態(tài)空間的目的是避免了“像Wiener濾波器一樣需要對過去所有[0,n-1]時(shí)刻協(xié)方差先驗(yàn)知識都已知”，而直接可以通過上一時(shí)刻即n-1時(shí)刻的狀態(tài)信息和均方誤差信息就可遞推得到n時(shí)刻的估計(jì)。盡管遞推使得實(shí)際應(yīng)用中方便了，但n-1對n時(shí)刻的估計(jì)實(shí)際上使用到了所有前[0,n-1]時(shí)刻的信息，只不過信息一直通過最小均方誤差進(jìn)行傳遞到n-1時(shí)刻。基于此，Kalman濾波也需要先驗(yàn)知識，即-1時(shí)刻的初始值。

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

Kalman濾波器巧妙的用“獨(dú)立高斯分布的乘積”將這兩個(gè)測量值和估計(jì)值進(jìn)行融合！

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

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

現(xiàn)在令

則(10)(11)變成：

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

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

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

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

高斯分布乘積運(yùn)算的結(jié)果仍為高斯分布，高斯分布的均值對應(yīng)n時(shí)刻的估計(jì)值，高斯分布的方差對應(yīng)n時(shí)刻的均方誤差。

Matlab程序看過來

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

% 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 the covariance % of that vector) based upon the information in a new observation. % The version of the Kalman filter in this function assumes that % observations occur at fixed discrete time intervals. Also, this % function assumes a linear system, meaning that the time evolution % of the state vector can be calculated by means of a state transition % matrix. % % USAGE: % % s = kalmanf(s) % % "s" is a "system" struct containing various fields used as input % and output. The state estimate "x" and its covariance "P" are % updated by the function. The other fields describe the mechanics % of the system and are left unchanged. A calling routine may change % these other fields as needed if state dynamics are time-dependent; % otherwise, they should be left alone after initial values are set. % The exceptions are the observation vectro "z" and the input control % (or forcing function) "u." If there is an input function, then % "u" should be set to some nonzero value by the calling routine. % % SYSTEM DYNAMICS: % % The system evolves according to the following difference equations, % where quantities are further defined below: % % x = Ax + Bu + w meaning the state vector x evolves during one time % step by premultiplying by the "state transition % matrix" A. There is optionally (if nonzero) an input % vector u which affects the state linearly, and this % linear effect on the state is represented by % premultiplying by the "input matrix" B. There is also % gaussian process noise w. % z = Hx + v meaning the observation vector z is a linear function % of the state vector, and this linear relationship is % represented by premultiplication by "observation % matrix" H. There is also gaussian measurement % noise v. % where w ~ N(0,Q) meaning w is gaussian noise with covariance Q % v ~ N(0,R) meaning v is gaussian noise with covariance R % % VECTOR VARIABLES: % % s.x = state vector estimate. In the input struct, this is the % "a priori" state estimate (prior to the addition of the % information from the new observation). In the output struct, % this is the "a posteriori" state estimate (after the new % measurement information is included). % s.z = observation vector % s.u = input control vector, optional (defaults to zero). % % MATRIX VARIABLES: % % s.A = state transition matrix (defaults to identity). % s.P = covariance of the state vector estimate. In the input struct, % this is "a priori," and in the output it is "a posteriori." % (required unless autoinitializing as described below). % s.B = input matrix, optional (defaults to zero). % s.Q = process noise covariance (defaults to zero). % s.R = measurement noise covariance (required). % s.H = observation matrix (defaults to identity). % % NORMAL OPERATION: % % (1) define all state definition fields: A,B,H,Q,R % (2) define intial state estimate: x,P % (3) obtain observation and control vectors: z,u % (4) call the filter to obtain updated state estimate: x,P % (5) return to step (3) and repeat % % INITIALIZATION: % % If an initial state estimate is unavailable, it can be obtained % from the first observation as follows, provided that there are the % same number of observable variables as state variables. This "auto- % intitialization" is done automatically if s.x is absent or NaN. % % x = inv(H)*z % P = inv(H)*R*inv(H') % % This is mathematically equivalent to setting the initial state estimate % covariance to infinity.function s = kalmanf(s)% set defaults for absent fields: if ~isfield(s,'x'); s.x=nan*z; end if ~isfield(s,'P'); s.P=nan; end if ~isfield(s,'z'); error('Observation vector missing'); end if ~isfield(s,'u'); s.u=0; end if ~isfield(s,'A'); s.A=eye(length(x)); end if ~isfield(s,'B'); s.B=0; end if ~isfield(s,'Q'); s.Q=zeros(length(x)); end if ~isfield(s,'R'); error('Observation covariance missing'); end if ~isfield(s,'H'); s.H=eye(length(x)); endif isnan(s.x)% initialize state estimate from first observationif diff(size(s.H))error('Observation matrix must be square and invertible for state autointialization.');ends.x = inv(s.H)*s.z;s.P = inv(s.H)*s.R*inv(s.H'); else% This is the code which implements the discrete Kalman filter:% Prediction for state vector and covariance:s.x = s.A*s.x + s.B*s.u;s.P = s.A * s.P * s.A' + s.Q;% Compute Kalman gain factor:K = s.P*s.H'*inv(s.H*s.P*s.H'+s.R);% Correction based on observation:s.x = s.x + K*(s.z-s.H*s.x);s.P = s.P - K*s.H*s.P;% Note that the desired result, which is an improved estimate% of the sytem state vector x and its covariance P, was obtained% in only five lines of code, once the system was defined. (That's% how simple the discrete Kalman filter is to use.) Later,% we'll discuss how to deal with nonlinear systems.endreturn

該程序中使用的符號的含義與本文一致，函數(shù)前的注釋再清晰不過了，就不多說。下面是一段測試代碼：

% Define the system as a constant of 12 volts: clear all s.x = 12; s.A = 1; % Define a process noise (stdev) of 2 volts as the car operates: s.Q = 2^2; % variance, hence stdev^2 % Define the voltimeter to measure the voltage itself: s.H = 1; % Define a measurement error (stdev) of 2 volts: s.R = 2^2; % variance, hence stdev^2 % Do not define any system input (control) functions: s.B = 0; s.u = 0; % Do not specify an initial state: s.x = nan; s.P = nan; % Generate random voltages and watch the filter operate. tru=[]; % truth voltage for t=1:20tru(end+1) = randn*2+12;s(end).z = tru(end) + randn*2; % create a measurements(end+1)=kalmanf(s(end)); % perform a Kalman filter iteration end figure hold on grid on % plot measurement data: hz=plot([s(1:end-1).z],'r.'); % plot a-posteriori state estimates: hk=plot([s(2:end).x],'b-'); ht=plot(tru,'g-'); legend([hz hk ht],'observations','Kalman output','true voltage',0) title('Automobile Voltimeter Example') hold off

Kalman的參數(shù)中s.Q和s.R的設(shè)置非常重要，之前也提過，一般要通過實(shí)驗(yàn)統(tǒng)計(jì)得到，它們分布代表了狀態(tài)空間估計(jì)的誤差和測量的誤差。

Kalman濾波器的效果是使輸出變得更平滑，但沒辦法去除信號中原有的椒鹽噪聲，而且，Kalman濾波器也會跟蹤這些椒鹽噪聲點(diǎn)，因此推薦在使用Kalman濾波器前先使用中值濾波去除椒鹽噪聲。

Kalman濾波C程序

我就在上面公式的基礎(chǔ)上實(shí)現(xiàn)了基本的Kalman濾波器，包括1維和2維狀態(tài)的情況。先在頭文件中聲明1維和2維Kalman濾波器結(jié)構(gòu)：

/** FileName : kalman_filter.h* Author : xiahouzuoxin @163.com* Version : v1.0* Date : 2014/9/24 20:37:01* Brief : * * Copyright (C) MICL,USTB*/ #ifndef _KALMAN_FILTER_H #define _KALMAN_FILTER_H/* * NOTES: n Dimension means the state is n dimension, * measurement always 1 dimension *//* 1 Dimension */ typedef struct {float x; /* state */float A; /* x(n)=A*x(n-1)+u(n),u(n)~N(0,q) */float H; /* z(n)=H*x(n)+w(n),w(n)~N(0,r) */float q; /* process(predict) noise convariance */float r; /* measure noise convariance */float p; /* estimated error convariance */float gain; } kalman1_state;/* 2 Dimension */ typedef struct {float x[2]; /* state: [0]-angle [1]-diffrence of angle, 2x1 */float A[2][2]; /* X(n)=A*X(n-1)+U(n),U(n)~N(0,q), 2x2 */float H[2]; /* Z(n)=H*X(n)+W(n),W(n)~N(0,r), 1x2 */float q[2]; /* process(predict) noise convariance,2x1 [q0,0; 0,q1] */float r; /* measure noise convariance */float p[2][2]; /* estimated error convariance,2x2 [p0 p1; p2 p3] */float gain[2]; /* 2x1 */ } kalman2_state; extern void kalman1_init(kalman1_state *state, float init_x, float init_p); extern float kalman1_filter(kalman1_state *state, float z_measure); extern void kalman2_init(kalman2_state *state, float *init_x, float (*init_p)[2]); extern float kalman2_filter(kalman2_state *state, float z_measure);#endif /*_KALMAN_FILTER_H*/

我都給了有詳細(xì)的注釋，kalman1_state是狀態(tài)空間為1維/測量空間1維的Kalman濾波器，kalman2_state是狀態(tài)空間為2維/測量空間1維的Kalman濾波器。兩個(gè)結(jié)構(gòu)體都需要通過初始化函數(shù)初始化相關(guān)參數(shù)、狀態(tài)值和均方差值。

/** FileName : kalman_filter.c* Author : xiahouzuoxin @163.com* Version : v1.0* Date : 2014/9/24 20:36:51* Brief : * * Copyright (C) MICL,USTB*/#include "kalman_filter.h"/** @brief * Init fields of structure @kalman1_state.* I make some defaults in this init function:* A = 1;* H = 1; * and @q,@r are valued after prior tests.** NOTES: Please change A,H,q,r according to your application.** @inputs * state - Klaman filter structure* init_x - initial x state value * init_p - initial estimated error convariance* @outputs * @retval */ void kalman1_init(kalman1_state *state, float init_x, float init_p) {state->x = init_x;state->p = init_p;state->A = 1;state->H = 1;state->q = 2e2;//10e-6; /* predict noise convariance */state->r = 5e2;//10e-5; /* measure error convariance */ }/** @brief * 1 Dimension Kalman filter* @inputs * state - Klaman filter structure* z_measure - Measure value* @outputs * @retval * Estimated result*/ float kalman1_filter(kalman1_state *state, float z_measure) {/* Predict */state->x = state->A * state->x;state->p = state->A * state->A * state->p + state->q; /* p(n|n-1)=A^2*p(n-1|n-1)+q *//* Measurement */state->gain = state->p * state->H / (state->p * state->H * state->H + state->r);state->x = state->x + state->gain * (z_measure - state->H * state->x);state->p = (1 - state->gain * state->H) * state->p;return state->x; }/** @brief * Init fields of structure @kalman1_state.* I make some defaults in this init function:* A = {{1, 0.1}, {0, 1}};* H = {1,0}; * and @q,@r are valued after prior tests. ** NOTES: Please change A,H,q,r according to your application.** @inputs * @outputs * @retval */ void kalman2_init(kalman2_state *state, float *init_x, float (*init_p)[2]) {state->x[0] = init_x[0];state->x[1] = init_x[1];state->p[0][0] = init_p[0][0];state->p[0][1] = init_p[0][1];state->p[1][0] = init_p[1][0];state->p[1][1] = init_p[1][1];//state->A = {{1, 0.1}, {0, 1}};state->A[0][0] = 1;state->A[0][1] = 0.1;state->A[1][0] = 0;state->A[1][1] = 1;//state->H = {1,0};state->H[0] = 1;state->H[1] = 0;//state->q = {{10e-6,0}, {0,10e-6}}; /* measure noise convariance */state->q[0] = 10e-7;state->q[1] = 10e-7;state->r = 10e-7; /* estimated error convariance */ }/** @brief * 2 Dimension kalman filter* @inputs * state - Klaman filter structure* z_measure - Measure value* @outputs * state->x[0] - Updated state value, Such as angle,velocity* state->x[1] - Updated state value, Such as diffrence angle, acceleration* state->p - Updated estimated error convatiance matrix* @retval * Return value is equals to state->x[0], so maybe angle or velocity.*/ float kalman2_filter(kalman2_state *state, float z_measure) {float temp0 = 0.0f;float temp1 = 0.0f;float temp = 0.0f;/* Step1: Predict */state->x[0] = state->A[0][0] * state->x[0] + state->A[0][1] * state->x[1];state->x[1] = state->A[1][0] * state->x[0] + state->A[1][1] * state->x[1];/* p(n|n-1)=A^2*p(n-1|n-1)+q */state->p[0][0] = state->A[0][0] * state->p[0][0] + state->A[0][1] * state->p[1][0] + state->q[0];state->p[0][1] = state->A[0][0] * state->p[0][1] + state->A[1][1] * state->p[1][1];state->p[1][0] = state->A[1][0] * state->p[0][0] + state->A[0][1] * state->p[1][0];state->p[1][1] = state->A[1][0] * state->p[0][1] + state->A[1][1] * state->p[1][1] + state->q[1];/* Step2: Measurement *//* gain = p * H^T * [r + H * p * H^T]^(-1), H^T means transpose. */temp0 = state->p[0][0] * state->H[0] + state->p[0][1] * state->H[1];temp1 = state->p[1][0] * state->H[0] + state->p[1][1] * state->H[1];temp = state->r + state->H[0] * temp0 + state->H[1] * temp1;state->gain[0] = temp0 / temp;state->gain[1] = temp1 / temp;/* x(n|n) = x(n|n-1) + gain(n) * [z_measure - H(n)*x(n|n-1)]*/temp = state->H[0] * state->x[0] + state->H[1] * state->x[1];state->x[0] = state->x[0] + state->gain[0] * (z_measure - temp); state->x[1] = state->x[1] + state->gain[1] * (z_measure - temp);/* Update @p: p(n|n) = [I - gain * H] * p(n|n-1) */state->p[0][0] = (1 - state->gain[0] * state->H[0]) * state->p[0][0];state->p[0][1] = (1 - state->gain[0] * state->H[1]) * state->p[0][1];state->p[1][0] = (1 - state->gain[1] * state->H[0]) * state->p[1][0];state->p[1][1] = (1 - state->gain[1] * state->H[1]) * state->p[1][1];return state->x[0]; }

其實(shí)，Kalman濾波器由于其遞推特性，實(shí)現(xiàn)起來很簡單。但調(diào)參有很多可研究的地方，主要需要設(shè)定的參數(shù)如下：

init_x：待測量的初始值，如有中值一般設(shè)成中值（如陀螺儀）

init_p：后驗(yàn)狀態(tài)估計(jì)值誤差的方差的初始值

q：預(yù)測（過程）噪聲方差

r：測量（觀測）噪聲方差。以陀螺儀為例，測試方法是：保持陀螺儀不動(dòng)，統(tǒng)計(jì)一段時(shí)間內(nèi)的陀螺儀輸出數(shù)據(jù)。數(shù)據(jù)會近似正態(tài)分布，按3σ原則，取正態(tài)分布的(3σ)^2作為r的初始化值。

其中q和r參數(shù)尤為重要，一般得通過實(shí)驗(yàn)測試得到。

找兩組聲陣列測向的角度數(shù)據(jù)，對上面的C程序進(jìn)行測試。一維Kalman（一維也是標(biāo)量的情況，就我所知，現(xiàn)在網(wǎng)上看到的代碼大都是使用標(biāo)量的情況）和二維Kalman（一個(gè)狀態(tài)是角度值，另一個(gè)狀態(tài)是向量角度差，也就是角速度）的結(jié)果都在圖中顯示。這里再稍微提醒一下：狀態(tài)量不要取那些能突變的量，如加速度，這點(diǎn)在文章“從牛頓到卡爾曼”一小節(jié)就提到過。

Matlab繪出的跟蹤結(jié)果顯示：

Kalman濾波結(jié)果比原信號更平滑。但是有椒鹽突變噪聲的地方，Kalman濾波器并不能濾除椒鹽噪聲的影響，也會跟蹤椒鹽噪聲點(diǎn)。因此，推薦在Kalman濾波器之前先使用中值濾波算法去除椒鹽突變點(diǎn)的影響。

上面所有C程序的源代碼及測試程序都公布在我的Github上，希望大家批評指正其中可能存在的錯(cuò)誤。

參考資料

Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation. Ramsey Faragher, Lecture Notes.

An Introduction to the Kalman Filter. Greg Welch and Gary Bishop.

http://robotsforroboticists.com/kalman-filtering/公式彩色著色，含pyhton源碼

http://alumni.media.mit.edu/~wad/mas864/psrc/kalman.c.txt包含Kalman濾波的C代碼

http://www.cs.unc.edu/~welch/kalman/比較全的Kalman鏈接

頂

總結(jié)

以上是生活随笔為你收集整理的kalman的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。