預估器
我們希望能夠最大限度地使用測量結果來預計移動物體的運動。
所以，多個測量的累積能夠讓我們檢測出不受噪聲影響的部分觀測軌跡。
一個關鍵的附加要素即此移動物體運動的模型。
有了這個模型，我們不僅能夠知道該移動物體在什么位置，同一時候還能夠知道我們觀察支持模型的什么參數。
該任務分為兩個階段。在第一階段，即預測階段。用從過去得到的信息進一步修正模型以取得人或物體的下一個將對出現的位置。在第二階段，即校正階段，我們獲得一個測量。然后與基於前一次測量的預測值（即模型）進行調整。完畢兩個階段預計任務的方法能夠成為預估器。
關於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)，過程噪聲協方差矩陣，測量噪聲協方差矩陣，后驗錯誤協方差矩陣，前一狀態校正后的值，當前觀察值。
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)