卡爾曼濾波器

1 簡介(Brief Introduction)

在學習卡爾曼濾波器之前，首先看看為什麼叫“卡爾曼”。跟其他著名的理論（例如傅立葉變換，泰勒級數等等）一樣，卡爾曼也是一個人的名字，而跟他們不同的是，他是個現代人！

卡爾曼全名Rudolf Emil Kalman，匈牙利數學家，1930年出生於匈牙利首都布達佩斯。1953，1954年於麻省理工學院分別獲得電機工程學士及碩士學位。1957年於哥倫比亞大學獲得博士學位。我們現在要學習的卡爾曼濾波器，正是源於他的博士論文和1960年發表的論文《A New Approach to Linear Filtering and Prediction Problems》（線性濾波與預測問題的新方法）。如果對這編論文有興趣，可以到這裡的地址下載閱讀：

卡爾曼濾波器到底是幹嘛的？我們來看下wiki上的解釋：

卡爾曼濾波的一個典型例項是從一組有限的，包含噪聲的，對物體位置的觀察序列（可能有偏差）預測出物體的位置的座標及速度。在很多工程應用(如雷達、計算機視覺)中都可以找到它的身影。同時，卡爾曼濾波也是控制理論以及控制系統工程中的一個重要課題。例如,對於雷達來說，人們感興趣的是其能夠跟蹤目標。但目標的位置、速度、加速度的測量值往往在任何時候都有噪聲。卡爾曼濾波利用目標的動態資訊，設法去掉噪聲的影響，得到一個關於目標位置的好的估計。這個估計可以是對當前目標位置的估計(濾波)，也可以是對於將來位置的估計(預測)，也可以是對過去位置的估計(插值或平滑)。

斯坦利.施密特(Stanley Schmidt)首次實現了卡爾曼濾波器。卡爾曼在NASA埃姆斯研究中心訪問時，發現他的方法對於解決阿波羅計劃的軌道預測很有用，後來阿波羅飛船的導航電腦便使用了這種濾波器。 關於這種濾波器的論文由Swerling (1958)、Kalman (1960)與 Kalman and Bucy (1961)發表。

目前,卡爾曼濾波已經有很多不同的實現.卡爾曼最初提出的形式現在一般稱為簡單卡爾曼濾波器。除此以外，還有施密特擴充套件濾波器、資訊濾波器以及很多Bierman, Thornton 開發的平方根濾波器的變種。也許最常見的卡爾曼濾波器是鎖相環，它在收音機、計算機和幾乎任何視訊或通訊裝置中廣泛存在。

簡單來說，卡爾曼濾波器是一個“optimal recursive data processing algorithm（最優化自迴歸資料處理演算法）”。對於解決很大部分的問題，他是最優，效率最高甚至是最有用的。他的廣泛應用已經超過30年，包括機器人導航，控制，感測器資料融合甚至在軍事方面的雷達系統以及導彈追蹤等等。近年來更被應用於計算機影象處理，例如頭臉識別，影象分割，影象邊緣檢測等等。

說起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年完成的，至於到底誰是Boss，矮子們就別管了吧！

在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濾波器方程。

目前，卡爾曼濾波已經有很多不同的實現。卡爾曼最初提出的形式現在一般稱為簡單卡爾曼濾波器。除此以外，還有施密特擴充套件濾波器、資訊濾波器以及很多Bierman, Thornton開發的平方根濾波器的變種。也許最常見的卡爾曼濾波器是鎖相環，它在收音機、計算機和幾乎任何視訊或通訊裝置中廣泛存在。

2 卡爾曼濾波器的介紹 (Introduction to the Kalman Filter)

為了可以更加容易的理解卡爾曼濾波器，這裡會應用形象的描述方法來講解，而不是像大多數參考書那樣羅列一大堆的數學公式和數學符號。
但是，他的5條公式是其核心內容。結合現代的計算機，其實卡爾曼的程式相當的簡單，只要你理解了他的那5條公式。

在介紹他的5條公式之前，先讓我們來根據下面的例子一步一步的探索。

假設我們要研究的物件是一個房間的溫度。根據你的經驗判斷，這個房間的溫度是恆定的，也就是下一分鐘的溫度等於現在這一分鐘的溫度(假設我們用一分鐘來做時間單位)。假設你對你的經驗不是100%的相信，可能會有上下偏差幾度。我們把這些偏差看成是高斯白噪聲(White Gaussian Noise)，也就是這些偏差跟前後時間是沒有關係的而且符合高斯分配(Gaussian Distribution)。另外，我們在房間裡放一個溫度計，但是這個溫度計也不準確的，測量值會比實際值偏差。我們也把這些偏差看成是高斯白噪聲。

好了，現在對於某一分鐘我們有兩個有關於該房間的溫度值：你根據經驗的預測值(系統的預測值)和溫度計的值(測量值)。下面我們要用這兩個值結合他們各自的噪聲來估算出房間的實際溫度值。

假如我們要估算k時刻的是實際溫度值。首先你要根據k-1時刻的溫度值，來預測k時刻的溫度。因為你相信溫度是恆定的，所以你會得到k時刻的溫度預測值是跟k-1時刻一樣的，假設是23度，同時該值的高斯噪聲的偏差是5度(5是這樣得到的：如果k-1時刻估算出的最優溫度值的偏差是3，你對自己預測的不確定度是4度，他們平方相加再開方，就是5)。然後，你從溫度計那裡得到了k時刻的溫度值，假設是25度，同時該值的偏差是4度。

由於我們用於估算k時刻的實際溫度有兩個溫度值，分別是23度和25度。究竟實際溫度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點，我們可以用他們的covariance(協方差)來判斷。因為Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我們可以估算出k時刻的實際溫度值是：23+0.78*(25-23)=24.56度。可以看出，因為溫度計的covariance比較小(比較相信溫度計)，所以估算出的最優溫度值偏向溫度計的值。

現在我們已經得到k時刻的最優溫度值了，下一步就是要進入k+1時刻，進行新的最優估算。到現在為止，好像還沒看到什麼自迴歸的東西出現。對了，在進入k+1時刻之前，我們還要算出k時刻那個最優值(24.56度)的偏差。演算法如下：((1-Kg)*5^2)^0.5=2.35。這裡的5就是上面的k時刻你預測的那個23度溫度值的偏差，得出的2.35就是進入k+1時刻以後k時刻估算出的最優溫度值的偏差(對應於上面的3)。

就是這樣，卡爾曼濾波器就不斷的把covariance遞迴，從而估算出最優的溫度值。他執行的很快，而且它只保留了上一時刻的covariance。上面的Kg，就是卡爾曼增益(Kalman Gain)。他可以隨不同的時刻而改變他自己的值，是不是很神奇！

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

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

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

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

下面就要言歸正傳，討論真正工程系統上的卡爾曼。

3 卡爾曼濾波器演算法 (The Kalman Filter Algorithm)

在這一部分，我們就來描述源於Dr. Kalman 的卡爾曼濾波器。

下面的描述，會涉及一些基本的概念知識，包括概率(Probability)，隨即變數(Random Variable)，高斯或正態分配(Gaussian Distribution)還有State-space Model等等。
但對於卡爾曼濾波器的詳細證明，這裡不能一一描述。

首先，我們先要引入一個離散控制過程的系統。
該系統可用一個線性隨機微分方程(Linear Stochastic Difference equation)來描述：X(k)=A X(k-1)+B U(k)+W(k), 再加上系統的測量值：Z(k)=H X(k)+V(k).

在上兩式子中，X(k)是k時刻的系統狀態，U(k)是k時刻對系統的控制量。A和B是系統引數，對於多模型系統，他們為矩陣。
Z(k)是k時刻的測量值，H是測量系統的引數，對於多測量系統，H為矩陣。W(k)和V(k)分別表示過程和測量的噪聲。
他們被假設成高斯白噪聲(White Gaussian Noise)，他們的covariance 分別是Q，R(這裡我們假設他們不隨系統狀態變化而變化)。

對於滿足上面的條件(線性隨機微分系統，過程和測量都是高斯白噪聲)，卡爾曼濾波器是最優的資訊處理器。

下面我們來用他們結合他們的covariances 來估算系統的最優化輸出(類似上一節那個溫度的例子)。

首先我們要利用系統的過程模型，來預測下一狀態的系統。假設現在的系統狀態是k，根據系統的模型，可以基於系統的上一狀態而預測出現在狀態：

X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)

式(1)中，X(k|k-1)是利用上一狀態預測的結果，X(k-1|k-1)是上一狀態最優的結果，U(k)為現在狀態的控制量，如果沒有控制量，它可以為0。

到現在為止，我們的系統結果已經更新了，可是，對應於X(k|k-1)的covariance還沒更新。我們用P表示covariance：

P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)

式(2)中，P(k|k-1)是X(k|k-1)對應的covariance，P(k-1|k-1)是X(k-1|k-1)對應的covariance，A’表示A的轉置矩陣，Q是系統過程的covariance。

式子1，2就是卡爾曼濾波器5個公式當中的前兩個，也就是對系統的預測。

現在我們有了現在狀態的預測結果，然後我們再收集現在狀態的測量值。結合預測值和測量值，我們可以得到現在 狀態(k) 的最優化估算值X(k|k)：

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)

其中Kg為 卡爾曼增益(Kalman Gain) ：

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

到現在為止，我們已經得到了k狀態下最優的估算值X(k|k)。但是為了要讓卡爾曼濾波器不斷的執行下去直到系統過程結束，我們還要更新k狀態下的X(k|k)的covariance：

P(k|k)=(I-Kg(k) H)P(k|k-1) ……… (5)

其中I 為1的矩陣，對於單模型單測量，I=1。當系統進入k+1狀態時，P(k|k)就是式子(2)的P(k-1|k-1)。這樣，演算法就可以自迴歸的運算下去。

卡爾曼濾波器的原理基本描述了，式子1，2，3，4和5就是他的5個基本公式。根據這5個公式，可以很容易的實現計算機的程式。

下面，我會用程式舉一個實際執行的例子。。。

4 簡單例子 (A Simple Example)

這裡我們結合第二第三節，舉一個非常簡單的例子來說明卡爾曼濾波器的工作過程。所舉的例子是進一步描述第二節的例子，而且還會配以程式模擬結果。

根據第二節的描述，把房間看成一個系統，然後對這個系統建模。當然，我們見的模型不需要非常地精確。我們所知道的這個房間的溫度是跟前一時刻的溫度相同的，所以A=1。沒有控制量，所以U(k)=0。因此得出
：
X(k|k-1)=X(k-1|k-1) ……….. (6)

式子(2)可以改成：

P(k|k-1)=P(k-1|k-1) +Q ……… (7)

因為測量的值是溫度計的，跟溫度直接對應，所以H=1。
式子3，4，5可以改成以下：

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)

Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)

P(k|k)=(1-Kg(k))P(k|k-1) ……… (10)

現在我們模擬一組測量值作為輸入。假設房間的真實溫度為25度，我模擬了200個測量值，這些測量值的平均值為25度，但是加入了標準偏差為幾度的高斯白噪聲(在圖中為藍線)。

為了令卡爾曼濾波器開始工作，我們需要告訴卡爾曼兩個零時刻的初始值，是X(0|0)和P(0|0)。他們的值不用太在意，隨便給一個就可以了，因為隨著卡爾曼的工作，X會逐漸的收斂。但是對於P，一般不要取0，因為這樣可能會令卡爾曼完全相信你給定的X(0|0)是系統最優的，從而使演算法不能收斂。我選了X(0|0)=1度，P(0|0)=10。

該系統的真實溫度為25度，圖中用黑線表示。圖中紅線是卡爾曼濾波器輸出的最優化結果(該結果在演算法中設定了Q=1e-6，R=1e-1)。

一段Matlab程式碼是從網上找到的，程式簡單直接，但作為學習分析用很棒:

% 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)); end

if isnan(s.x)
   % initialize state estimate from first observation
   if diff(size(s.H))
      error('Observation matrix must be square and invertible for state autointialization.');
   end
   s.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.

end

return

下面是一段測試程式碼：

% 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:20
   tru(end+1) = randn*2+12;
   s(end).z = tru(end) + randn*2; % create a measurement
   s(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濾波C程式

我就在上面公式的基礎上實現了基本的Kalman濾波器，包括1維和2維狀態的情況。先在標頭檔案中宣告1維和2維Kalman濾波器結構：

/*
 * 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*/

原始碼給了有詳細的註釋，kalman1_state是狀態空間為1維/測量空間1維的Kalman濾波器，kalman2_state是狀態空間為2維/測量空間1維的Kalman濾波器。

兩個結構體都需要通過初始化函式初始化相關引數、狀態值和均方差值。

/*
 * 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];
}

其實，Kalman濾波器由於其遞推特性，實現起來很簡單。但調參有很多可研究的地方，主要需要設定的引數如下：

init_x：待測量的初始值，如有中值一般設成中值（如陀螺儀）
init_p：後驗狀態估計值誤差的方差的初始值
q：預測（過程）噪聲方差
r：測量（觀測）噪聲方差。以陀螺儀為例，測試方法是：保持陀螺儀不動，統計一段時間內的陀螺儀輸出資料。資料會近似正態分佈，按3σ原則，取正態分佈的(3σ)^2作為r的初始化值。
其中q和r引數尤為重要，一般得通過實驗測試得到。

找兩組聲陣列測向的角度資料，對上面的C程式進行測試。一維Kalman（一維也是標量的情況，就我所知，現在網上看到的程式碼大都是使用標量的情況）和二維Kalman（一個狀態是角度值，另一個狀態是向量角度差，也就是角速度）的結果都在圖中顯示。這裡再稍微提醒一下：狀態量不要取那些能突變的量，如加速度.

上面所有C程式的原始碼及測試程式都公佈在原作者的Github上。