卡尔曼滤波（Kalman Filter）在目标边框预测中的应用

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

2. http://www.cs.unc.edu/~welch/kalman/