無人機目標跟蹤的原理原來是這樣計算的

今天給大家推薦是一篇關於計算機視覺方面的文章，主要內容為基於Kalman濾波器介紹及基於其進行物體的跟蹤的相關模型，最後給出相關平台實現參考。

對做無人機運動目標跟蹤及機器人相關領域的學習者與開發者能夠有一個導向作用。

預估器

我們希望可以最大限度地使用測量結果來估計移動物體的運動。所以，多個測量的累積可以讓我們檢測出不受雜訊影響的部分觀測軌跡。一個關鍵的附加要素即此移動物體運動的模型。有了這個模型，我們不僅可以知道該移動物體在什麼位置，同時還可以知道我們觀察支持模型的什麼參數。

該任務分為兩個階段。在第一階段，即預測階段，用從過去得到的信息進一步修正模型以取得人或物體的下一個將對出現的位置。在第二階段，即校正階段，我們獲得一個測量，然後與基於前一次測量的預測值（即模型）進行調整。完成兩個階段估計任務的方法可以成為預估器。

關於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 濾波包括兩個階段：預測和更新；在估計階段，濾波器應用上一狀態的估計做出對當前狀態的估計。在更新階段，濾波器利用在當前狀態的觀測值優化預測階段的預測值，以獲的一個更精確的當前狀態的估計。

預測

更新

基本概念圖示

使用OpenCV中Kalman編程的主要步驟

步驟一Kalman這個類需要初始化變數：

轉移矩陣，測量矩陣，控制向量(沒有的話，就是0)，過程雜訊協方差矩陣，測量雜訊協方差矩陣，後驗錯誤協方差矩陣，前一狀態校正後的值，當前觀察值。

步驟三：

調用kalman這個類的correct方法得到加入觀察值校正後的狀態變數值矩陣

其公式為：

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

（本文轉自無窮創新）

推薦閱讀：