卡爾曼濾波器跟蹤
(What is the Kalman Filter?)
在學習卡爾曼濾波器之前,首先看看為什麼叫“卡爾曼”。跟其他著名的理論(例如傅立葉變換,泰勒級數等等)一樣,卡爾曼也是一個人的名字,而跟他們不同的是,他是個現代人!
卡爾曼全名Rudolf Emil Kalman,匈牙利數學家,1930年出生於匈牙利首都布達佩斯。1953,1954年於麻省理工學院分別獲得電機工程學士及碩士學位。1957年於哥倫比亞大學獲得博士學位。我們現在要學習的卡爾曼濾波器,正是源於他的博士論文和1960年發表的論文《A New Approach to Linear Filtering and Prediction Problems》(線性濾波與預測問題的新方法)。如果對這編論文有興趣,可以到這裡的地址下載:
簡單來說,卡爾曼濾波器是一個“optimal recursive data processing algorithm(最優化自迴歸資料處理演算法)”。對於解決很大部分的問題,他是最優,效率最高甚至是最有用的。他的廣泛應用已經超過30年,包括機器人導航,控制,感測器資料融合甚至在軍事方面的雷達系統以及導彈追蹤等等。近年來更被應用於計算機影象處理,例如頭臉識別,影象分割,影象邊緣檢測等等。
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)。他可以隨不同的時刻而改變他自己的值,是不是很神奇!
下面就要言歸正傳,討論真正工程系統上的卡爾曼。
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)。
一些網路資料
關於Kalman濾波器的理論,其數學公式太多,大家可以去檢視一些這方面的文獻.下面這篇文章對Kalman濾波做了個通俗易懂的介紹,通過文章舉的例子可以巨集觀上理解一下該濾波器,很不錯,推薦一看:http://www.cnblogs.com/feisky/archive/2009/11/09/1599247.html,
中介紹了opencv1.0版本的卡爾曼濾波的結構和函式定義等。
kalman濾波簡單介紹
Kalman濾波理論主要應用在現實世界中個,並不是理想環境。主要是來跟蹤的某一個變數的值,跟蹤的依據是首先根據系統的運動方程來對該值做預測,比如說我們知道一個物體的運動速度,那麼下面時刻它的位置按照道理是可以預測出來的,不過該預測肯定有誤差,只能作為跟蹤的依據。另一個依據是可以用測量手段來測量那個變數的值,當然該測量也是有誤差的,也只能作為依據,不過這2個依據的權重比例不同。最後kalman濾波就是利用這兩個依據進行一些列迭代進行目標跟蹤的。
在這個理論框架中,有2個公式一定要懂,即:
第一個方程為系統的運動方程,第二個方程為系統的觀測方程,學過自控原理中的現代控制理論的同學應該對這2個公式很熟悉。具體的相關理論本文就不做介紹了。
Opencv目標版本中帶有kalman這個類,可以使用它來完成一些跟蹤目的。
下面來看看使用Kalman程式設計的主要步驟:
步驟一 :
Kalman這個類需要初始化下面變數:
轉移矩陣,測量矩陣,控制向量(沒有的話,就是0),過程噪聲協方差矩陣,測量噪聲協方差矩陣,後驗錯誤協方差矩陣,前一狀態校正後的值,當前觀察值。
步驟二:
呼叫kalman這個類的predict方法得到狀態的預測值矩陣,預測狀態的計算公式如下:
predicted state (x'(k)): x'(k)=A*x(k-1)+B*u(k)
其中x(k-1)為前一狀態的校正值,第一個迴圈中在初始化過程中已經給定了,後面的迴圈中Kalman這個類內部會計算。A,B,u(k),也都是給定了的值。這樣進過計算就得到了系統狀態的預測值x'(k)了。
步驟三:
呼叫kalman這個類的correct方法得到加入觀察值校正後的狀態變數值矩陣,其公式為:
corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
其中x'(k)為步驟二算出的結果,z(k)為當前測量值,是我們外部測量後輸入的向量。H為Kalman類初始化給定的測量矩陣。K(k)為Kalman增益,其計算公式為:
Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
計算該增益所依賴的變數要麼初始化中給定,要麼在kalman理論中通過其它公式可以計算。
經過步驟三後,我們又重新獲得了這一時刻的校正值,後面就不斷迴圈步驟二和步驟三即可完成Kalman濾波過程。
實驗部分
本次實驗來源於opencv自帶sample中的例子,該例子是用kalman來完成一個一維的跟蹤,即跟蹤一個不斷變化的角度。在介面中表現為一個點在圓周上勻速跑,然後跟蹤該點。看起來跟蹤點是個二維的,其實轉換成角度就是一維的了。
該程式碼中,有這麼幾句
KalmanFilter KF(2, 1, 0);
...
KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 1, 0, 1);
...
一直在想Mat_<float>(2, 2) << 1是什麼意思呢?
如果是用:Mat_<float> A(2, 2);則這表示的是定義一個矩陣A。
但是Mat_<float>(2, 2)感覺又不是定義,貌似也不是數,既然不是數怎能左移呢?後面發現自己想錯了.
Mat_<float>(2, 2) << 1, 1, 0, 1是一個整體,即往Mat_<float>(2, 2)的矩陣中賦值1,1,0,1;說白了就是給Mat矩陣賦初值,因為初值沒什麼規律,所以我們不能用zeros,ones等手段來賦值。比如執行下面語句時:
Mat a;
a = (Mat_<float>(2, 2) << 1, 1, 0, 1);
cout<<a<<endl;
其結果就為
[1,1;
0,1]
實驗結果如下:
跟蹤中某一時刻圖1:
跟蹤中某一時刻圖2:
其中紅色的短線條為目標點真實位置和目標點的測量位置的連線,黃色的短線為目標點真實位置和預測位置的連線,所以2中顏色相交中間那個點座標為目標的真實座標。
實驗主要部分程式碼和註釋(附錄有工程code下載連結地址):
Kalman.h:
#ifndef KALMAN_H #define KALMAN_H #include <QDialog> #include <QTimer> #include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/video/video.hpp> using namespace cv; namespace Ui { class kalman; } class kalman : public QDialog { Q_OBJECT public: explicit kalman(QWidget *parent = 0); ~kalman(); private slots: void on_startButton_clicked(); void kalman_process(); void on_closeButton_clicked(); private: Ui::kalman *ui; Mat img, state, processNoise, measurement; KalmanFilter KF; QTimer *timer; //給定圓心和周長,和圓周上的角度,求圓周上的座標 static inline Point calcPoint(Point2f center, double R, double angle) { //sin前面有個負號那是因為圖片的頂點座標在左上角 return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R; } }; #endif // KALMAN_H
Kalman.cpp:
#include "kalman.h" #include "ui_kalman.h" #include <iostream> using namespace std; kalman::kalman(QWidget *parent) : QDialog(parent), ui(new Ui::kalman) { //在建構函式中定義變數不行? img.create(500, 500, CV_8UC3); cout<<img.rows<<endl; //狀態維數2,測量維數1,沒有控制量 KF.init(2, 1, 0); //狀態值,(角度,角速度) state.create(2, 1, CV_32F); processNoise.create(2, 1, CV_32F); measurement = Mat::zeros(1, 1, CV_32F); timer = new QTimer(this); connect(timer, SIGNAL(timeout()), this, SLOT(kalman_process())); ui->setupUi(this); //這句必須放在ui->setupUi(this)後面,因為只有這樣才能訪問ui->textBrowser ui->textBrowser->setFixedSize(500, 500); } kalman::~kalman() { delete ui; } void kalman::on_startButton_clicked() { /* 使用kalma步驟一 下面語句到for前都是kalman的初始化過程,一般在使用kalman這個類時需要初始化的值有: 轉移矩陣,測量矩陣,過程噪聲協方差,測量噪聲協方差,後驗錯誤協方差矩陣, 前一狀態校正後的值,當前觀察值 */ //產生均值為0,標準差0.1的二維高斯列向量 randn( state, Scalar::all(0), Scalar::all(0.1) ); //transitionMatrix為類KalmanFilter中的一個變數,Mat型,是Kalman模型中的狀態轉移矩陣 //轉移矩陣為[1,1;0,1],2*2維的 KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 1, 0, 1); //函式setIdentity是給引數矩陣對角線賦相同值,預設對角線值值為1 setIdentity(KF.measurementMatrix); //系統過程噪聲方差矩陣 setIdentity(KF.processNoiseCov, Scalar::all(1e-5)); //測量過程噪聲方差矩陣 setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1)); //後驗錯誤估計協方差矩陣 setIdentity(KF.errorCovPost, Scalar::all(1)); //statePost為校正狀態,其本質就是前一時刻的狀態 randn(KF.statePost, Scalar::all(0), Scalar::all(0.1)); //設定定時器時間,預設情況下其該定時器可無限定時,即其SingleShot為false //如果將其設定為true,則該定時器只能定時一次 //因此這裡是每個33ms就去執行一次kalman處理函式 timer->start(33); // timer->setSingleShot( true ); } void kalman::kalman_process() { Point2f center(img.cols*0.5f, img.rows*0.5f); float R = img.cols/3.f; //state中存放起始角,state為初始狀態 double stateAngle = state.at<float>(0); Point statePt = calcPoint(center, R, stateAngle); /* 使用kalma步驟二 呼叫kalman這個類的predict方法得到狀態的預測值矩陣 */ //predicted state (x'(k)): x'(k)=A*x(k-1)+B*u(k) //其中x(k-1)為前面的校正狀態,因此這裡是用校正狀態來預測的 //而校正狀態corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) //又與本時刻的預測值和校正值有關係 Mat prediction = KF.predict(); //用kalman預測的是角度 double predictAngle = prediction.at<float>(0); Point predictPt = calcPoint(center, R, predictAngle); randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0))); // generate measurement //帶噪聲的測量 measurement += KF.measurementMatrix*state; // measurement += KF.measurementMatrix*prediction; double measAngle = measurement.at<float>(0); Point measPt = calcPoint(center, R, measAngle); // plot points //這個define語句是畫2條線段(線長很短),其實就是畫一個“X”叉符號 #define drawCross( center, color, d ) \ line( img, Point( center.x - d, center.y - d ), \ Point( center.x + d, center.y + d ), color, 1, CV_AA, 0); \ line( img, Point( center.x + d, center.y - d ), \ Point( center.x - d, center.y + d ), color, 1, CV_AA, 0 ) img = Scalar::all(0); //狀態座標白色 drawCross( statePt, Scalar(255,255,255), 3 ); //測量座標藍色 drawCross( measPt, Scalar(0,0,255), 3 ); //預測座標綠色 drawCross( predictPt, Scalar(0,255,0), 3 ); //真實值和測量值之間用紅色線連線起來 line( img, statePt, measPt, Scalar(0,0,255), 3, CV_AA, 0 ); //真實值和估計值之間用黃色線連線起來 line( img, statePt, predictPt, Scalar(0,255,255), 3, CV_AA, 0 ); /* 使用kalma步驟三 呼叫kalman這個類的correct方法得到加入觀察值校正後的狀態變數值矩陣 */ //雖然該函式有返回值Mat,但是呼叫該函式校正後,其校正值已經儲存在KF的statePost //中了,corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) KF.correct(measurement); randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0)))); //不加噪聲的話就是勻速圓周運動,加了點噪聲類似勻速圓周運動,因為噪聲的原因,運動方向可能會改變 state = KF.transitionMatrix*state + processNoise; imwrite("../kalman/img.jpg", img); ui->textBrowser->clear(); //ui->textBrowser->setFixedSize(img.cols, img.rows); u