Kalman濾波器從原理到實現

轉載請註明出處：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

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

說起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開發的平方根濾波器的變種。也許最常見的卡爾曼濾波器是鎖相環，它在收音機、計算機和幾乎任何視頻或通訊設備中廣泛存在。

從牛頓到卡爾曼

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

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

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

那麼就有下面的位移和速度關係：

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

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

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

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

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

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

從運動的角度，很容易理解：小車當前n時刻的位移和速度一部分來自於n-1時刻的慣性作用，這通過Ax(n)來度量，另一部分來自於現在n時刻小車新增加的外部受力，通過Bu(n)來度量。
w(n)是過程噪聲，w(n)~N(0,Q)的高斯分佈，過程噪聲是使用卡爾曼濾波器時一個重要的量，後面會進行分析。

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

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

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

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

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

一場遞推的遊戲

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

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

預測

預測值：

最小均方誤差矩陣：
修正

誤差增益：

修正值：

這裏的A去掉，參見下面的圖

最小均方誤差矩陣：

從(5)~(9)中：

x(n)：Nx1的狀態矢量
z(n)：Mx1的觀測矢量，Kalman濾波器的輸入
x(n|n-1)：用n時刻以前的數據進行對n時刻的估計結果
x(n|n)：用n時刻及n時刻以前的數據對n時刻的估計結果，這也是Kalman濾波器的輸出
P(n|n-1)：NxN，最小預測均方誤差矩陣，其定義式爲

通過計算最終得到(6)式。
P(n|n)：NxN，修正後最小均方誤差矩陣。
K(n)：NxM，誤差增益，從增益的表達式看，相當於“預測最小均方誤差”除以“n時刻的測量誤差+預測最小均方誤差”，直觀含義就是用n-1預測n時刻狀態的預測最小均方誤差在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狀態的位移，速度爲v=10m/s，n時刻與n-1時刻的物理時差爲ΔT=1s。同時，也用尺子測了一下，結果位移爲z(n)=62m。

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

。

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

要求已知上次的修正後的最小均方誤差P(n-1|n-1)=[1 0; 0 0]（勻速，所以P(2,2)=0，右斜對角線上爲協方差，值爲0，P(1,1)爲n-1時刻位移量的均方誤差，因爲要計算P(1,1)還得先遞推往前計算P(n-2|n-2)，所以這裏暫時假設爲1），則根據(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時刻的初始值，然後在預測（狀態空間）和修正（觀測空間）之間不停的遞推，求取n時刻的估計x和均方誤差矩陣P。

均方誤差中的門道

到這裏，應該對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濾波器巧妙的用“獨立高斯分佈的乘積”將這兩個測量值和估計值進行融合！

如下圖：估計量的高斯分佈和測量量的高斯分佈經過融合後爲綠色的高斯分佈曲線。

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

現在令

則(10)(11)變成：

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

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

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

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

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

Matlab程序看過來

下面的一段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的參數中s.Q和s.R的設置非常重要，之前也提過，一般要通過實驗統計得到，它們分佈代表了狀態空間估計的誤差和測量的誤差。

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

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（一個狀態是角度值，另一個狀態是向量角度差，也就是角速度）的結果都在圖中顯示。這裏再稍微提醒一下：狀態量不要取那些能突變的量，如加速度，這點在文章“從牛頓到卡爾曼”一小節就提到過。

Matlab繪出的跟蹤結果顯示：

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

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

參考資料

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鏈接

頂27
踩

Kalman濾波器從原理到實現

Kalman濾波器的歷史淵源

從牛頓到卡爾曼

一場遞推的遊戲

均方誤差中的門道

Matlab程序看過來

Kalman濾波C程序

參考資料

DSP程序開發與優化經驗之二：程序運行時間測量方法總結

Jquery和JS獲取ul中li標籤的實現方法

小程序開發之 wx.getSetting 授權檢測

ThinkPHP5 數據庫助手函數模型(model)的使用

小程序View設置背景顏色漸變+透明+陰影【轉載】

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結