【電腦視覺】基於Kalman濾波器的進行物體的跟蹤

標籤：kalman濾波器   opencv   

預估器

我們希望可以最大限度地使用測量結果來估計移動物體的運動。所以，多個測量的累積可以讓我們檢測出不受雜訊影響的部分觀測軌跡。一個關鍵的附加要素即此移動物體運動的模型。有了這個模型，我們不僅可以知道該移動物體在什麼位置，同時還可以知道我們觀察支援模型的什麼參數。
該任務分為兩個階段。在第一階段，即預測階段，用從過去得到的資訊進一步修正模型以取得人或物體的下一個將對出現的位置。在第二階段，即校正階段，我們獲得一個測量，然後與基於前一次測量的預測值（即模型）進行調整。完成兩個階段估計任務的方法可以成為預估器。

關於Kalman濾波的通俗解釋

在介紹Kalman濾波器原理之前，先從一個通俗的例子中瞭解一下該演算法的功能和作用，以便更好的理解它。以下是摘自網上的一段資料。
假設我們要研究的對象是一個房間的溫度。根據你的經驗判斷，這個房間的溫度是恒定的，也就是下一分鐘的溫度等於現在這一分鐘的溫度（假設我們用一分鐘來做時間單位）。假設你對你的經驗不是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=5^2/(5^2+4^2)，所以Kg=0.61，我們可以估算出k時刻的實際溫度值是：23+0.61(25-23)=24.22度。可以看出，因為溫度計的covariance比較小（比較相信溫度計），所以估算出的最優溫度值偏向溫度計的值。
現在我們已經得到k時刻的最優溫度值了，下一步就是要進入k+1時刻，進行新的最優估算。到現在為止，好像還沒看到什麼自迴歸的東西出現。對了，在進入k+1時刻之前，我們還要算出k時刻那個最優值（24.22度）的偏差。演算法如下：((1-Kg)5^2)^0.5=3.12。這裡的5就是上面的k時刻你預測的那個23度溫度值的偏差，得出的3.12就是進入k+1時刻以後k時刻估算出的最優溫度值的偏差（對應於上面的3）。
就是這樣，卡爾曼濾波器就不斷的把covariance遞迴，從而估算出最優的溫度值。他啟動並執行很快，而且它只保留了上一時刻的covariance。上面的Kg，就是卡爾曼增益（Kalman Gain）。他可以隨不同的時刻而改變他自己的值，是不是很神奇！

Kalman濾波器簡介

Kalman（卡爾曼）濾波是一種高效率的遞迴濾波器(自迴歸濾波器), 它能夠從一系列的不完全及包含雜訊的測量中，估計動態系統的狀態。Kalman濾波的一個典型執行個體是從一組有限的，包含雜訊的，對物體位置的觀察序列（可能有偏差）預測出物體的位置的座標及速度。
Kalman濾波器的基本思想是，若有一組強而合理（合理的意思是“限制非常寬鬆使得這種方法對真實世界中出現的相當多的實際問題都有用”）的假設，給出系統的曆史測量值，則可以建立最大化這些早前測量值的後驗機率的系統狀態模型。
另外，無需儲存很長的早前測量曆史，我們也可以最大化後驗機率，即重複更新系統狀態模型，並只為下一次更新儲存模型。

應用執行個體

一個簡單的應用是估計物體的位置和速度；簡要描述如下：假設我們可以擷取一個物體的包含雜訊的一系列位置觀測資料，我們可以獲得此物體的精確速度和位置連續更新資訊。
例如，對於雷達來說，我們關心的是跟蹤目標，而目標的位置，速度，加速度的測量值是時刻含有誤差的，卡爾曼濾波器利用目標的動態資訊，去掉雜訊影響，擷取目標此刻好的位置估計(濾波)，將來位置估計(預測)，也可以是過去位置估計的(插值或平滑)。

三個重要假設

Kalman濾波器需要三個重要假設：

• 被建模的系統是線性
• 影響測量的雜訊屬於白色雜訊
• 雜訊本質上是高斯分布的

第一條假設的意思是k時刻的系統狀態可以用某個矩陣與k-1時刻的系統狀態的乘積表示。餘下兩條假設，即假設雜訊是高斯分布的白色雜訊，其含義為雜訊與時間不相關，且只用均值和共變數（雜訊完全由一階矩和二階矩描述）就可以準確地為幅值建模。
給定三條假設，Kalman濾波器是將從不同來源擷取的資料或從統一來源不同時間獲得的資料結合的最好的方法。從我們知道的資訊開始，擷取新的資訊，然後根據對舊資訊和新資訊的確定程度，用新舊資訊帶權重的結合對我們知道的資訊進行更新。

Kalman濾波器的數學知識基本動態模型

卡爾曼濾波建立線上性代數和隱馬爾可夫模型(hidden Markov model)上。其基本動態系統可以用一個馬爾可夫鏈(Markov Chain)表示，該馬爾可夫鏈建立在一個被高斯雜訊(即常態分佈的雜訊)幹擾的線性運算元上的。系統的狀態可以用一個元素為實數的向量表示。 隨著離散時間的每一個增加,這個線性運算元就會作用在目前狀態上，產生一個新的狀態,並也會帶入一些雜訊,同時系統的一些已知的控制器的控制資訊也會被加入。同時，另一個受雜訊幹擾的線性運算元產生出這些隱含狀態的可見輸出。

Kalman 濾波可以被看作為類似隱馬爾科夫模型，它們的顯著不同點在於：隱狀態變數的取值空間是一個連續的空間，而離散狀態空間則不是；另為，隱馬爾科夫模型可以描述下一個狀態的一個任意分布，這也與應用於Kalman濾波器中的高斯雜訊模型相反。

為了從一系列的雜訊觀測中，應用Kalman濾波估計觀測過程的內部狀態。我們必須把這個過程在Kalman濾波器的架構下建立模型， 這就意味著，對於每一步k 我們要定義:
假設Kalman濾波器的k時刻的真實狀態時從k-1時刻眼花而來，
滿足X[k] = F[k]x[k-1]+B[k]u[k]+w[k]

• F[k]是作用在前一狀態的狀態轉移模型(狀態轉移矩陣)
• B[k]是作用在控制向量u[k]上的控制輸入模型(輸入輸出矩陣)，u[k]的作用是允許外部控制施加於系統
• w[k]是過程雜訊，假設是均值為0的白色雜訊，共變數為Q[k]，則w[k] ~ N(0,Q[k])

在k時刻，假設真實狀態x[k]的觀測，
Z[k]滿足公式Z[k] = H[k]x[k]+v[k]

• H[k]是觀測模型(觀測矩陣)，它把真實狀態映射到觀測空間
• v[k]是觀測雜訊，假設它是均值是0，方差是R[k]的高斯白色雜訊，v[k] ~ N(0,R[k])

模型圖：

動態系統模型

Kalman Filter基本動態系統模型如所示，其中，圓圈代表向量，方塊代表矩陣，星號代表高斯雜訊，其共變數在右下方標出。

• 初始狀態以及每一時刻的雜訊向量{x0, w1, ..., wk, v1 ... vk} 都為認為是互相獨立的。
• 實際中，真實世界中動態系統並不是嚴格的符合此模型，但是Kalman模型是設計在雜訊過程工作的，一個近似的符合已經可以使這個濾波器非常有用。

Kalman模型三種運動

在Kalman濾波器應用中，我們將考慮三種運動。
動態運動
這種運動時我們期望的前次測量時系統狀態的直接結果。
控制運動
這種運動是我們期望的，由於某種已知的外部因素以某種原因施加於系統。控制運動最普遍的一個例子是，當對我們施加了控制的系統估計其狀態時，我們知道我們的控制會使系統產生什麼樣的運行。
隨機運動
即便是最簡單的一維情況，如果觀測的目標有因任一原因而產生運動的可能性，那麼就需要在預測階段包含這種隨機運動。這種隨機運動影響的是簡單地增加狀態估計隨時間的共變數。

公式

Kalman濾波器是一個遞迴的估計，即只要獲知上一時刻的狀態估計和目前狀態的觀測就可以計算出目前狀態的估計，不同於其他的估計技術，Kalman濾波器不需要觀測或/和估計的記錄，Kalman濾波器是一個純粹的時域濾波器，而不像低通濾波器等頻域濾波器那樣，需要在頻域中設計，然後轉換到時域中應用。

Kalman變數

Kalman 濾波包括兩個階段：預測和更新；在估計階段，濾波器應用上一狀態的估計做出對目前狀態的估計。在更新階段，濾波器利用在目前狀態的觀測值最佳化預測階段的預測值，以獲的一個更精確的目前狀態的估計。

預測
預測公式更新
更新公式基本概念圖示
Kalman基本概念使用OpenCV中Kalman編程的主要步驟步驟一

Kalman這個類需要初始設定變數：
轉移矩陣，測量矩陣，控制向量(沒有的話，就是0)，過程雜訊共變數矩陣，測量雜訊共變數矩陣，後驗錯誤共變數矩陣，前一狀態校正後的值，當前觀察值。

void KalmanFilter::init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F)//Parameters:    //dynamParams – Dimensionality of the state.//measureParams – Dimensionality of the measurement.//controlParams – Dimensionality of the control vector.//type – Type of the created matrices that should be CV_32F or CV_64F.

步驟二

調用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)了。

const Mat& KalmanFilter::predict(const Mat& control=Mat())//Parameters:    control – The optional input control

步驟三：

調用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理論中通過其它公式可以計算。

const Mat& KalmanFilter::correct(const Mat& measurement)//Parameters:    measurement – The measured system parameters

經過步驟三後，我們又重新獲得了這一時刻的校正值，後面就不斷迴圈步驟二和步驟三即可完成Kalman濾波過程。

參考資料

1、學習OpenCV 中文版
2、學習OpenCV——Kalman濾波

轉載請註明作者Jason Ding及其出處
Github首頁(http://jasonding1354.github.io/)
CSDN部落格(http://blog.csdn.net/jasonding1354)
簡書首頁(http://www.jianshu.com/users/2bd9b48f6ea8/latest_articles)

【電腦視覺】基於Kalman濾波器的進行物體的跟蹤