預(yù)估器

我們希望可以最大限度地使用測量結(jié)果來估計移動物體的運動九秀。所以吐葱，多個測量的累積可以讓我們檢測出不受噪聲影響的部分觀測軌跡。一個關(guān)鍵的附加要素即此移動物體運動的模型。有了這個模型咙崎，我們不僅可以知道該移動物體在什么位置，同時還可以知道我們觀察支持模型的什么參數(shù)吨拍。
該任務(wù)分為兩個階段褪猛。在第一階段，即預(yù)測階段羹饰，用從過去得到的信息進(jìn)一步修正模型以取得人或物體的下一個將對出現(xiàn)的位置伊滋。在第二階段，即校正階段队秩，我們獲得一個測量笑旺，然后與基于前一次測量的預(yù)測值（即模型）進(jìn)行調(diào)整。完成兩個階段估計任務(wù)的方法可以成為預(yù)估器馍资。

關(guān)于Kalman濾波的通俗解釋

在介紹Kalman濾波器原理之前筒主，先從一個通俗的例子中了解一下該算法的功能和作用，以便更好的理解它鸟蟹。以下是摘自網(wǎng)上的一段資料乌妙。
假設(shè)我們要研究的對象是一個房間的溫度。根據(jù)你的經(jīng)驗判斷建钥，這個房間的溫度是恒定的藤韵，也就是下一分鐘的溫度等于現(xiàn)在這一分鐘的溫度（假設(shè)我們用一分鐘來做時間單位）。假設(shè)你對你的經(jīng)驗不是100%的相信熊经，可能會有上下偏差幾度泽艘。我們把這些偏差看成是高斯白噪聲（White Gaussian Noise），也就是這些偏差跟前后時間是沒有關(guān)系的而且符合高斯分布（Gaussian Distribution）奈搜。另外悉盆，我們在房間里放一個溫度計，但是這個溫度計也不準(zhǔn)確的馋吗，測量值會比實際值偏差焕盟。我們也把這些偏差看成是高斯白噪聲。
好了宏粤，現(xiàn)在對于某一分鐘我們有兩個有關(guān)于該房間的溫度值：你根據(jù)經(jīng)驗的預(yù)測值（系統(tǒng)的預(yù)測值）和溫度計的值（測量值）脚翘。下面我們要用這兩個值結(jié)合他們各自的噪聲來估算出房間的實際溫度值。
假如我們要估算k時刻的實際溫度值绍哎。首先你要根據(jù)k-1時刻的溫度值来农，來預(yù)測k時刻的溫度。因為你相信溫度是恒定的崇堰，所以你會得到k時刻的溫度預(yù)測值是跟k-1時刻一樣的沃于，假設(shè)是23度涩咖，同時該值的高斯噪聲的偏差是5度（5是這樣得到的：如果k-1時刻估算出的最優(yōu)溫度值的偏差是3，你對自己預(yù)測的不確定度是4度繁莹，他們平方相加再開方檩互，就是5）。然后咨演，你從溫度計那里得到了k時刻的溫度值闸昨，假設(shè)是25度，同時該值的偏差是4度薄风。
由于我們用于估算k時刻的實際溫度有兩個溫度值饵较，分別是23度和25度。究竟實際溫度是多少呢遭赂？相信自己還是相信溫度計呢循诉？究竟相信誰多一點，我們可以用他們的covariance來判斷撇他。因為Kg=5^2/(52+4^2)打洼，所以Kg=0.61，我們可以估算出k時刻的實際溫度值是：23+0.61(25-23)=24.22度逆粹∧即可以看出，因為溫度計的covariance比較衅У（比較相信溫度計）阿浓，所以估算出的最優(yōu)溫度值偏向溫度計的值。
現(xiàn)在我們已經(jīng)得到k時刻的最優(yōu)溫度值了蹋绽，下一步就是要進(jìn)入k+1時刻芭毙，進(jìn)行新的最優(yōu)估算。到現(xiàn)在為止卸耘，好像還沒看到什么自回歸的東西出現(xiàn)退敦。對了，在進(jìn)入k+1時刻之前蚣抗，我們還要算出k時刻那個最優(yōu)值（24.22度）的偏差侈百。算法如下：((1-Kg)5²⁾0.5=3.12。這里的5就是上面的k時刻你預(yù)測的那個23度溫度值的偏差翰铡，得出的3.12就是進(jìn)入k+1時刻以后k時刻估算出的最優(yōu)溫度值的偏差（對應(yīng)于上面的3）钝域。
就是這樣，卡爾曼濾波器就不斷的把covariance遞歸锭魔，從而估算出最優(yōu)的溫度值例证。他運行的很快，而且它只保留了上一時刻的covariance迷捧。上面的Kg织咧，就是卡爾曼增益（Kalman Gain）胀葱。他可以隨不同的時刻而改變他自己的值，是不是很神奇笙蒙！

Kalman濾波器簡介

Kalman（卡爾曼）濾波是一種高效率的遞歸濾波器(自回歸濾波器), 它能夠從一系列的不完全及包含噪聲的測量中巡社，估計動態(tài)系統(tǒng)的狀態(tài)。Kalman濾波的一個典型實例是從一組有限的手趣，包含噪聲的，對物體位置的觀察序列（可能有偏差）預(yù)測出物體的位置的坐標(biāo)及速度肥荔。
Kalman濾波器的基本思想是绿渣，若有一組強而合理（合理的意思是“限制非常寬松使得這種方法對真實世界中出現(xiàn)的相當(dāng)多的實際問題都有用”）的假設(shè)，給出系統(tǒng)的歷史測量值燕耿，則可以建立最大化這些早前測量值的后驗概率的系統(tǒng)狀態(tài)模型中符。
另外，無需存儲很長的早前測量歷史誉帅，我們也可以最大化后驗概率淀散，即重復(fù)更新系統(tǒng)狀態(tài)模型，并只為下一次更新保存模型蚜锨。

應(yīng)用實例

一個簡單的應(yīng)用是估計物體的位置和速度档插；簡要描述如下：假設(shè)我們可以獲取一個物體的包含噪聲的一系列位置觀測數(shù)據(jù)，我們可以獲得此物體的精確速度和位置連續(xù)更新信息亚再。
例如郭膛，對于雷達(dá)來說，我們關(guān)心的是跟蹤目標(biāo)氛悬，而目標(biāo)的位置则剃，速度，加速度的測量值是時刻含有誤差的如捅，卡爾曼濾波器利用目標(biāo)的動態(tài)信息棍现，去掉噪聲影響，獲取目標(biāo)此刻好的位置估計(濾波)镜遣，將來位置估計(預(yù)測)己肮，也可以是過去位置估計的(插值或平滑)。

三個重要假設(shè)

Kalman濾波器需要三個重要假設(shè)：

• 被建模的系統(tǒng)是線性的
• 影響測量的噪聲屬于白噪聲
• 噪聲本質(zhì)上是高斯分布的

第一條假設(shè)的意思是k時刻的系統(tǒng)狀態(tài)可以用某個矩陣與k-1時刻的系統(tǒng)狀態(tài)的乘積表示悲关。余下兩條假設(shè)朴肺，即假設(shè)噪聲是高斯分布的白噪聲，其含義為噪聲與時間不相關(guān)坚洽，且只用均值和協(xié)方差（噪聲完全由一階矩和二階矩描述）就可以準(zhǔn)確地為幅值建模戈稿。
給定三條假設(shè)，Kalman濾波器是將從不同來源獲取的數(shù)據(jù)或從統(tǒng)一來源不同時間獲得的數(shù)據(jù)結(jié)合的最好的方法讶舰。從我們知道的信息開始鞍盗，獲取新的信息需了，然后根據(jù)對舊信息和新信息的確定程度，用新舊信息帶權(quán)重的結(jié)合對我們知道的信息進(jìn)行更新般甲。

Kalman濾波器的數(shù)學(xué)知識

基本動態(tài)模型

卡爾曼濾波建立在線性代數(shù)和隱馬爾可夫模型(hidden Markov model)上肋乍。其基本動態(tài)系統(tǒng)可以用一個馬爾可夫鏈(Markov Chain)表示，該馬爾可夫鏈建立在一個被高斯噪聲(即正態(tài)分布的噪聲)干擾的線性算子上的敷存。系統(tǒng)的狀態(tài)可以用一個元素為實數(shù)的向量表示墓造。 隨著離散時間的每一個增加,這個線性算子就會作用在當(dāng)前狀態(tài)上，產(chǎn)生一個新的狀態(tài),并也會帶入一些噪聲,同時系統(tǒng)的一些已知的控制器的控制信息也會被加入锚烦。同時觅闽，另一個受噪聲干擾的線性算子產(chǎn)生出這些隱含狀態(tài)的可見輸出。

Kalman 濾波可以被看作為類似隱馬爾科夫模型涮俄，它們的顯著不同點在于：隱狀態(tài)變量的取值空間是一個連續(xù)的空間蛉拙，而離散狀態(tài)空間則不是；另為彻亲，隱馬爾科夫模型可以描述下一個狀態(tài)的一個任意分布孕锄，這也與應(yīng)用于Kalman濾波器中的高斯噪聲模型相反。

為了從一系列的噪聲觀測中苞尝，應(yīng)用Kalman濾波估計觀測過程的內(nèi)部狀態(tài)畸肆。我們必須把這個過程在Kalman濾波器的框架下建立模型， 這就意味著宙址，對于每一步k 我們要定義:
假設(shè)Kalman濾波器的k時刻的真實狀態(tài)時從k-1時刻眼花而來恼除，
滿足X[k] = F[k]x[k-1]+B[k]u[k]+w[k]

• F[k]是作用在前一狀態(tài)的狀態(tài)轉(zhuǎn)移模型(狀態(tài)轉(zhuǎn)移矩陣)
• B[k]是作用在控制向量u[k]上的控制輸入模型(輸入輸出矩陣)，u[k]的作用是允許外部控制施加于系統(tǒng)
• w[k]是過程噪聲曼氛，假設(shè)是均值為0的白噪聲豁辉，協(xié)方差為Q[k]，則w[k] ~ N(0,Q[k])

在k時刻舀患，假設(shè)真實狀態(tài)x[k]的觀測徽级，
Z[k]滿足公式Z[k] = H[k]x[k]+v[k]

• H[k]是觀測模型(觀測矩陣)，它把真實狀態(tài)映射到觀測空間
• v[k]是觀測噪聲聊浅，假設(shè)它是均值是0餐抢，方差是R[k]的高斯白噪聲，v[k] ~ N(0,R[k])

模型圖：

動態(tài)系統(tǒng)模型

Kalman Filter基本動態(tài)系統(tǒng)模型如上圖所示低匙，其中旷痕，圓圈代表向量，方塊代表矩陣顽冶，星號代表高斯噪聲欺抗，其協(xié)方差在右下方標(biāo)出。

• 初始狀態(tài)以及每一時刻的噪聲向量{x0, w1, ..., wk, v1 ... vk} 都為認(rèn)為是互相獨立的强重。
• 實際中绞呈，真實世界中動態(tài)系統(tǒng)并不是嚴(yán)格的符合此模型贸人，但是Kalman模型是設(shè)計在噪聲過程工作的，一個近似的符合已經(jīng)可以使這個濾波器非常有用佃声。

Kalman模型

三種運動

在Kalman濾波器應(yīng)用中艺智，我們將考慮三種運動。
動態(tài)運動
這種運動時我們期望的前次測量時系統(tǒng)狀態(tài)的直接結(jié)果圾亏。
控制運動
這種運動是我們期望的十拣，由于某種已知的外部因素以某種原因施加于系統(tǒng)≈揪椋控制運動最普遍的一個例子是夭问，當(dāng)對我們施加了控制的系統(tǒng)估計其狀態(tài)時，我們知道我們的控制會使系統(tǒng)產(chǎn)生什么樣的運行弄跌。
隨機運動
即便是最簡單的一維情況，如果觀測的目標(biāo)有因任一原因而產(chǎn)生運動的可能性尝苇，那么就需要在預(yù)測階段包含這種隨機運動铛只。這種隨機運動影響的是簡單地增加狀態(tài)估計隨時間的協(xié)方差。

公式

Kalman濾波器是一個遞歸的估計糠溜，即只要獲知上一時刻的狀態(tài)估計和當(dāng)前狀態(tài)的觀測就可以計算出當(dāng)前狀態(tài)的估計淳玩，不同于其他的估計技術(shù)，Kalman濾波器不需要觀測或/和估計的歷史記錄非竿，Kalman濾波器是一個純粹的時域濾波器蜕着，而不像低通濾波器等頻域濾波器那樣，需要在頻域中設(shè)計红柱，然后轉(zhuǎn)換到時域中應(yīng)用承匣。

Kalman變量

Kalman 濾波包括兩個階段：預(yù)測和更新；在估計階段锤悄，濾波器應(yīng)用上一狀態(tài)的估計做出對當(dāng)前狀態(tài)的估計韧骗。在更新階段，濾波器利用在當(dāng)前狀態(tài)的觀測值優(yōu)化預(yù)測階段的預(yù)測值零聚，以獲的一個更精確的當(dāng)前狀態(tài)的估計袍暴。

預(yù)測

預(yù)測公式

更新

更新公式

基本概念圖示

Kalman基本概念

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

步驟一

Kalman這個類需要初始化變量：
轉(zhuǎn)移矩陣，測量矩陣隶症，控制向量(沒有的話政模，就是0)，過程噪聲協(xié)方差矩陣蚂会，測量噪聲協(xié)方差矩陣物独，后驗錯誤協(xié)方差矩陣不脯，前一狀態(tài)校正后的值，當(dāng)前觀察值桨昙。

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.

步驟二

調(diào)用kalman這個類的predict方法得到狀態(tài)的預(yù)測值矩陣
預(yù)測狀態(tài)的計算公式如下：
predicted state (x'(k)): x'(k)=A * x(k-1)+B * u(k)
其中x(k-1)為前一狀態(tài)的校正值，第一個循環(huán)中在初始化過程中已經(jīng)給定了合溺，后面的循環(huán)中Kalman這個類內(nèi)部會計算。A,B,u(k),也都是給定了的值。這樣進(jìn)過計算就得到了系統(tǒng)狀態(tài)的預(yù)測值x'(k)了芦缰。

const Mat& KalmanFilter::predict(const Mat& control=Mat())

//Parameters:   control – The optional input control

步驟三：

調(diào)用kalman這個類的correct方法得到加入觀察值校正后的狀態(tài)變量值矩陣
其公式為：
corrected state (x(k)): x(k)=x'(k)+K(k) * (z(k)-H * x'(k))
其中x'(k)為步驟二算出的結(jié)果，z(k)為當(dāng)前測量值枫慷，是我們外部測量后輸入的向量让蕾。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

經(jīng)過步驟三后誉裆，我們又重新獲得了這一時刻的校正值顿颅，后面就不斷循環(huán)步驟二和步驟三即可完成Kalman濾波過程。

參考資料

1足丢、學(xué)習(xí)OpenCV 中文版
2粱腻、學(xué)習(xí)OpenCV——Kalman濾波

轉(zhuǎn)載請注明作者Jason Ding及其出處
Github主頁(http://jasonding1354.github.io/)
CSDN博客(http://blog.csdn.net/jasonding1354)
簡書主頁(http://www.reibang.com/users/2bd9b48f6ea8/latest_articles)