1.卡爾曼濾波的導論
卡爾曼濾波器（Kalman Filter），是由匈牙利數學家Rudolf Emil Kalman發明，並以其名字命名。卡爾曼出生於1930年匈牙利首都布達佩斯。1953，1954年分別獲得麻省理工學院的電機工程學士以及碩士學位。1957年於哥倫比亞大學獲得博士學位。卡爾曼濾波器是其在博士期間的研究成果，他的博士論文是《A New Approach to Linear Filtering and Prediction Problem》[1]。
卡爾曼濾波器是一個最優化自回歸數據處理的算法，在很多時候它是解決大部分最優問題最優且效率最高的方法。廣泛應用已經超過了35年，如機器人導航，控制，傳感器數據融合甚至軍事方面的雷達系統目標跟蹤，人臉識別，圖像分割和邊緣檢測等。
1.1 例子
下面以一個關於溫度估計的例子(來源於網絡)來闡述卡爾曼方法的基本思想。
假設我們要研究的對象是一個房間的溫度。根據你的經驗判斷，這個房間的溫度是恆定的，也就是下一分鍾的溫度等於現在這一份中的溫度（假設我們用1分鍾來做時間單位）。假設你對你的經驗不是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度，究竟實際溫度是多少呢？相信自己的經驗還是相信溫度計呢？究竟誰的置信度高，我們可以通過協方差來判斷，即
，由此我們可以估算出k時刻的實際溫度值是：23+0.78*（25-23）=24.56。可以看出，溫度計的協方差比較小，因此我們選擇相信溫度計多些），所以估算出的最優溫度值偏向溫度計的值。
現在我們已經得到k時刻的最優溫度值了，下一步就是要進入k+1時刻，進行新的最優估算。到現在位置，好像還沒有看到什么自回歸的東西出現。對了，在進入k+1時刻之前，我們還要算出k時刻那個最優值（24.56）的偏差, 算法如下：
,這里的5就是上面k時刻你預測的那個23度溫度值的偏差（5），得出的2.35就是進入k+1時刻以后k時刻估算出的最優溫度值的偏差（對應於上面的3）。
如此循環下去，卡爾曼濾波器就不斷的把協方差遞歸，從而估算出最優的溫度值。此方法的運算速度很快，而且它只保留了上一個時刻的協方差。上面的kg，就是卡爾曼增益（Kalman Gain）。他可以隨不同時刻而改變他自己的值。
假設k+1時刻溫度計讀書時24度，那么k+1時刻的最優溫度估計為
。更新最優值的偏差誤差為
。
1.2 理論
接下來我們來描述卡爾曼濾波器的原理。下面的描述，會涉及一些基本的概念知識，包括概率，隨機變量，高斯分布還有狀態空間模型等。
首先，引入一個離散控制過程的系統。該系統可用一個線性隨機差分方程來描述：
其次，系統的測量值：
以上式子中，X(k)是k時刻的系統狀態，U(k)是k時刻對系統的控制量。A和B是系統參數，對於多模型系統，他們為矩陣。Z(k)是k時刻的測量值，H是測量系統的參數，對於多測量系統，H為矩陣。W(k)和V(k)分別表示過程和測量的噪聲。他們被假設成高斯白噪聲，他們的協方差分別是Q，R(這里我們假設他們不隨系統狀態的變化而變化)。
對於滿足上面的條件，卡爾曼濾波器是最優的信息處理器。下面我們來用他們結合他們的協方差來估算系統的最優輸出。
首先我們要利用系統的過程模型，來預測下一個狀態的系統。假設現在的系統狀態是k，根據系統的模型，可以基於系統的上一個狀態而預測出現在的狀態：
式子（a）中，
是利用上一狀態預測的結果，
是上一狀態最優的結果，U(k)為現在狀態的控制量，如果沒有控制量，它可以為0.
到現在位置，我們的系統結果已經更新了，可是，對應於
的協方差還沒有更新。我們用P表示協方差：
式（b）中，
是
對應的協方差，P(k-1|k-1)是X（k-1|k-1）對應的協方差，A'表示A的轉置矩陣，Q表示系統過程的協方差。式子a,b就是卡爾曼濾波器五條公式中的2條，也就是對系統的預測。
現在我們有了現在狀態的預測結果，然后我們再收集現在狀態的測量值。結合預測值和測量值，我們可以得到現在狀態（k）的最優化估計值X(k|k):
其中Kg為卡爾曼增益（Kalman Gain）：
到現在為止，我們已經得到了k狀態下最優的估算值X（k|k）。但是為了要卡爾曼濾波器不斷的運行下去知道系統結束，我們還要更新k狀態下X（k|k）的協方差：
其中I為1的矩陣，對於單模型單測量，I=1,。當系統進入k+1狀態時，P（k|k）就是式子（b）的
.這樣算法就可以遞歸下去了。
卡爾曼濾波器的原理就是由式子（a）-（e）五條基本公式組成。下面以卡爾曼濾波器在目標邊框預測中的應用進行建模和代碼實現
2. 卡爾曼濾波在目標邊框預測中的應用
2.1 建模
假設已知t-1時刻，目標邊框
, 
分別表示t-1時刻邊框左上角x,y坐標點，右下角x，y坐標點。假設每條邊以均勻速度變化，即
。同時知道t時刻下，
的觀測量。現在我們需要預測t時刻下，邊框的最優邊框預測。邊框如下圖所示
系統狀態表示為
,其元素分別表示邊框的左上角頂點的x，y和右下角頂點的x，y，以及它們的變化量。系統狀態方程中的A可表示為：
由於沒有控制量，所以B=0，W為高斯白噪聲，設置為0。則系統狀態方程(a)可表示為：
(a')
先驗協方差可表示為：
(b')
當k=1時，P（0）我們設置為：
是系統過程的協方差，我們設置為：
接下來可以計算卡爾曼增益：
其中
得到卡爾曼增益后，那么最優系統狀態應為：
接下來更新X(k)的協方差，更新公式為：
如此循環下去，就可以不斷的預測目標邊框的值了。
2.2基於OpenCV的實現
MotionFilter.h
#pragma once
#include "opencv2/opencv.hpp"
#include "opencv2/highgui/highgui.hpp"
using namespace cv;
enum MotionFilterType{KALMAN_FILTER = 0, CONDENSATION_FILTER = 1};//濾波器類型
#ifndef CSST_CAMMOV_STATE
#define CSST_CAMMOV_STATE
enum CamMovStat{CMS_MOVING, CMS_STOP, CMS_UNCERTEN};
#endif
//相機狀態
#ifndef CSST_CAMSTATE
#define CSST_CAMSTATE
class CamState
{
public:
float x;//x移動方向
float y;//y移動方向
float step;//移動補償
CamMovStat state;//相機狀態
CamState(float x=0., float y=0., float step=0.,CamMovStat state=CamMovStat::CMS_STOP)
{
this->x = x;
this->y = y;
this->state = state;
this->step = step;
}
};
#endif
class CMotionFilter
{
public:
CMotionFilter();
virtual ~CMotionFilter(void);
// 初始化運動濾波跟蹤
virtual bool Init(cv::Rect& inBBox) = 0;
// 更新運動濾波器，獲得新的目標邊框
virtual int Update(cv::Rect& inBBox, const CamState& inCamState) = 0;
// 獲取濾波器類型，返回卡爾曼 或者Cendensation
virtual MotionFilterType GetFilterType(void) = 0;
};
MothonFilter.cpp
#include "MotionFilter.h"
CMotionFilter::CMotionFilter()
{
}
CMotionFilter::~CMotionFilter(void)
{
}
KalmanMotionFilter.h
#pragma once
#include "motionfilter.h"
class CKalmanMotionFilter :
public CMotionFilter
{
public:
CKalmanMotionFilter();
~CKalmanMotionFilter(void);
// 初始化運動濾波跟蹤
virtual bool Init(cv::Rect& inBBox);
// 更新運動濾波器，獲得新的目標邊框
virtual int Update(cv::Rect& inBBox, const CamState& inCamState);
// 獲取濾波器類型，返回卡爾曼
virtual MotionFilterType GetFilterType(void);
void CalcCorrectPos(const CamState camst, float &xC, float &yC);
private:
KalmanFilter m_Kalman;
int m_KalmanIsInit;
cv::Mat m_Measurement ;//Z(k)
cv::Mat m_Realposition ;//real X(k)
cv::Mat m_ControlMatrix;
int m_LastIdx;
Rect m_LastBox;
int m_StopFrameCount;
CamState m_LastCamState;//相機上一幀的狀態
int m_CamStateCount;//用於計算相依移動修正量的權重 ，與mLastCamState變量 和 CalcCorrectPos函數配套使用
};
KalmanMotionFilter.cpp
#include "KalmanMotionFilter.h"
CKalmanMotionFilter::CKalmanMotionFilter(){
}
CKalmanMotionFilter::~CKalmanMotionFilter(void)
{
}
// 初始化運動濾波跟蹤
bool CKalmanMotionFilter::Init(cv::Rect& inBBox)
{
m_Kalman.init(8,4,0);
m_Kalman.transitionMatrix = *(Mat_<float>(8, 8) << 1,0,0,0,1,0,0,0, 0,1,0,0,0,1,0,0, 0,0,1,0,0,0,1,0, 0,0,0,1,0,0,0,1,
0,0,0,0,1,0,0,0, 0,0,0,0,0,1,0,0, 0,0,0,0,0,0,1,0, 0,0,0,0,0,0,0,1);
setIdentity(m_Kalman.measurementMatrix,Scalar::all(1));
setIdentity(m_Kalman.processNoiseCov,Scalar::all(1e-5));
setIdentity(m_Kalman.measurementNoiseCov,Scalar::all(1e-1));
setIdentity(m_Kalman.errorCovPost,Scalar::all(1));
m_Measurement = cvCreateMat( 4, 1, CV_32FC1 );//Z(k)
m_Realposition = cvCreateMat( 8, 1, CV_32FC1 );//real X(k)
m_ControlMatrix = cvCreateMat( 8, 1, CV_32FC1 );//real X(k)
m_LastBox = inBBox;
m_Kalman.statePost = *(Mat_<float>(8,1) << inBBox.x, inBBox.y, inBBox.br().x-1, inBBox.br().y-1,0 ,0 , 0, 0);
m_KalmanIsInit = 1;
m_LastCamState = CamMovStat::CMS_STOP;
return false;
}
// 更新運動濾波器，獲得新的目標邊框
int CKalmanMotionFilter::Update(cv::Rect& inBBox, const CamState& inCamState = CamState())
{
float xCorrect = 0.0;
float yCorrect = 0.0;
CalcCorrectPos(inCamState,xCorrect, yCorrect);
//當相機運動的時,后驗狀態概率進行修正，修正量為相機的移動速度。
m_Kalman.statePost.at<float>(4,0)=m_Kalman.statePost.at<float>(4,0)+xCorrect;
m_Kalman.statePost.at<float>(5,0)=m_Kalman.statePost.at<float>(5,0)+yCorrect;
m_Kalman.statePost.at<float>(6,0)=m_Kalman.statePost.at<float>(6,0)+xCorrect;
m_Kalman.statePost.at<float>(7,0)=m_Kalman.statePost.at<float>(7,0)+yCorrect;
//相機運動的話會輸入控制矩陣進行修正。
Mat PredictPoint = m_Kalman.predict();//預測
int obj_x = cvRound(PredictPoint.at<float>(0,0));
int obj_y = cvRound(PredictPoint.at<float>(1,0));
int obj_x1 = cvRound(PredictPoint.at<float>(2,0));
int obj_y1 = cvRound(PredictPoint.at<float>(3,0));
Rect meansureBox = Rect(inBBox.x,inBBox.y,inBBox.br().x - 1,inBBox.br().y - 1);
m_Realposition.at<float>(0,0)=meansureBox.x;
m_Realposition.at<float>(1,0)=meansureBox.y;
m_Realposition.at<float>(2,0)=meansureBox.width;
m_Realposition.at<float>(3,0)=meansureBox.height;
m_Realposition.at<float>(4,0)=meansureBox.x - m_LastBox.x + 1;
m_Realposition.at<float>(5,0)=meansureBox.y- m_LastBox.y + 1;
m_Realposition.at<float>(6,0)=meansureBox.width- m_LastBox.br().x + 1;
m_Realposition.at<float>(7,0)=meansureBox.height- m_LastBox.br().y + 1;
m_Measurement = m_Kalman.measurementMatrix*m_Realposition;
m_Kalman.correct(m_Measurement);//修正誤差
//mLastBox = meansureBox;
inBBox.x = obj_x;
inBBox.y = obj_y;
inBBox.width = obj_x1 - obj_x + 1;
inBBox.height = obj_y1 - obj_y + 1;
m_LastBox = inBBox;
return 0;
}
// 獲取濾波器類型，返回卡爾曼
MotionFilterType CKalmanMotionFilter::GetFilterType(void)
{
return MotionFilterType::KALMAN_FILTER;
}
//計算對目標的位置補償，用於對卡爾曼預測結果進行運動補償
void CKalmanMotionFilter::CalcCorrectPos(const CamState camst, float &xC, float &yC)
{
if (CamMovStat::CMS_MOVING == camst.state)
{
if (CamMovStat::CMS_STOP ==m_LastCamState.state && CamMovStat::CMS_MOVING == camst.state)
{//開始對bbox進行向后運動補償計算
m_CamStateCount = 0;
m_LastCamState.x = camst.x;
m_LastCamState.y = camst.y;
}
double sigm = 1;
double weight = exp(-1.0*m_CamStateCount*m_CamStateCount/(sigm*sigm));
xC = m_LastCamState.x*weight;
yC = m_LastCamState.y*weight;
m_CamStateCount++;
}
if (CamMovStat::CMS_STOP == camst.state)
{
if (CamMovStat::CMS_MOVING ==m_LastCamState.state && CamMovStat::CMS_STOP == camst.state)
{//開始對bbox進行向后運動補償計算
m_CamStateCount = 0;
}
double sigm = 1;
double weight = exp(-1.0*m_CamStateCount*m_CamStateCount/(sigm*sigm));
xC = -m_LastCamState.x*weight;
yC = -m_LastCamState.y*weight;
m_CamStateCount++;
}
m_LastCamState.state = camst.state;
}
word版本：卡爾曼濾波（KalmanFilter）在目標邊框預測中的應用.zip
參考文獻
1. Rudolf Emil Kalman. "A New Approach to Linear Filtering and Prediction Problems" .http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf. 1960