Published at:   |   Reading: 2059 words ~5min   |   PV/UV: /

前段时间做Tracking，了解到Kalman滤波，感觉很不错。wikipedia上内容有点多，整理过后发现只有五个核心公式。

原理

Kalman滤波器不断把协方差递归，估算出最优值，并且保留了上一时刻的协方差（Kalman增益），因此它的运行很快，可以随不同的时刻而改变他的值。 首先，我们要引入一个离散控制过程系统，该系统可以用一个线性随机微分方程来描述：

$$ \begin{equation} X(k)=AX(k-1)+BU(k)+W(k) \end{equation} $$

加上系统的测量值：

$$ \begin{equation} Z(k)=HX(k)+V(k) \end{equation} $$

上面式子中，$X(k)$是$k$时刻的系统状态，$U(k)$是$k$时刻对系统的控制量。$AB$是系统参数，对于多模型系统，他们为矩阵。$Z(k)$是$k$时刻的测量值，$H$是测量系统的参数，对于多测量系统，$H$为矩阵。

$W(k)$和$V(k)$分别表示过程和测量的噪声。他们被假设成高斯白噪声他们的协方差分别为$Q$，$R$（假设不随系统状态变化而变化）。 对于满足上面条件（线性随机微分系统，过程和测量都是高斯白噪声），Kalman滤波器是最优的信息处理器。下面则用其自身和自身的协方差来估计系统的最优化输出。 首先，要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统上一状态而预测出现在状态：

$$ \begin{equation} X(k|k-1)=AX(k-1|k-1)+BU(k) \end{equation} $$

上式中，$X(k|k-1)$是利用上一状态预测的结果，$X(k-1|k-1)$是上一状态最优的结果，$U(k)$为现在状态的控制量，如果没有控制量，它可以为0。 至此，系统结果已经更新，但对于$X(k|k-1)$的协方差还未更新，用$Cov$表示协方差则：

$$ \begin{equation} Cov(k|k-1)=ACov(k-1|k-1)A^{T}+Q \end{equation} $$

上式中，$Cov(k|k-1)$是$X(k|k-1)$对应的协方差，$Cov(k-1|k-1)$是$X(k-1|k-1)$对应的协方差，$A^{T}$表示$A$的转置，$Q$是系统过程的协方差。上面两个式子为对系统的预测。 当有了现在状态的预测结果，我们再收集现在状态的预测值。结合预测值和测量值，我们可以得到现在状态$k$的最优化估计值$X(k|k)$：

$$ \begin{equation} X(k|k)=X(k|k-1)+K_{g}(k)(Z(k)-HX(k|k-1)) \end{equation} $$

其中，$K_{g}$为Kalman增益：

$$ \begin{equation} K_{g}=Cov(k|k-1)H^{T}(HCov(k|k-1)H^{T}+R(k))^{-1} \end{equation} $$

于此，已经得到了k状态下的最优估算值$X(k|k)$。但是为了要另Kalman滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下$X(k|k)$的协方差：

$$ \begin{equation} Cov(k|k)=(E-K_{g}H)Cov(k|k-1) \end{equation} $$

其中，$E$为单位矩阵。当系统进入$k+1$状态时，$Cov(k|k)$就是第二个公式的，因此，可以自回归下去。

应用

Kalman滤波器作为一个很好的预测模型，可以根据现有情况来预测未来情况，在做Tracking中则变得很有用处。对于一个对象$S$的Tracking，$S$的pos在域中漂移，利用Tracking可以使得pos平滑稳定。 在OpenCV中提供了CvKalman结构，但是使用起来仍然需要建立模型，前几天我根据上述原理对整个过程进行了封装：

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52

//
//  CvKalmanPoint.h
//  Renovation
//
//  Version History: 
//		Created by 欧 长坤 on 2013-08-17 
//
//  Description：
//		//2013-08-17
//		创建CvKalmanPoint对象并使用
//		cvKalmanPositionPrediction方法来返回Kalman滤波预测点。
//  
//  Copyright (c) 2013年 欧 长坤. All rights reserved.
//
//
#ifndef CVKALMANPOINT_H__
#define CVKALMANPOINT_H__

#include <opencv/cv.h>
#include <opencv/cxcore.h>
#include <opencv/highgui.h>

class CvKalmanPoint
{
public:

	// 参数
	int stateNum;
	int measureNum;

	// 屏幕分辨率
	int windowHeight;
	int windowWidth;

	CvKalman* kalman;
	CvMat* process_noise;
	CvMat* measurement;
	CvRNG rng;

	// 鼠标坐标
	CvPoint mousePosition;

	// 初始化
	CvKalmanPoint(void);
	~CvKalmanPoint(void);

	// 预测卡尔曼点
	CvPoint cvKalmanPositionPrediction(void);

};

#endif

下面则是构造函数和cvKalmanPositionPrediction方法的具体实现：

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92

#include "CvKalmanPoint.h"
#include <windows.h>
#include <winuser.h>

CvKalmanPoint::CvKalmanPoint(void)
{

	windowWidth  = GetSystemMetrics(SM_CXSCREEN);
	windowHeight = GetSystemMetrics(SM_CYSCREEN); 

	stateNum  	  = 4;		// 状态数，包括(x,y,dx,dy)坐标及速度（每次移动的距离）
	measureNum	  = 2;		// 实际量，能看到的是坐标值，当然也可以自己计算速度，但是没有必要。 
	kalman	 	  = cvCreateKalman( stateNum, measureNum, 0 );	// state(x,y,detaX,detaY)  
	process_noise = cvCreateMat( stateNum, 1, CV_32FC1 );
	measurement	  = cvCreateMat( measureNum, 1, CV_32FC1 );		// measurement(x,y) 
	rng			  = cvRNG(-1);	// 64位长整数种子，初始化rng

	/*
		// 变换矩阵
		┌ x'┐  ┌1 0 1 0┐┌ x┐
		│ y'│= │0 1 0 1││ y│
		│dx'│  │0 0 1 0││dx│
		└dy'┘  └0 0 0 1┘└dy┘
		==> x'  = x + dx
		==> y'  = y + dy
		==> dx' = dx
		==> dy' = dy
	*/
	// 4 == stateNum
	float transformMatrix[4][4] = {  
		1,0,1,0,  
		0,1,0,1,  
		0,0,1,0,  
		0,0,0,1  
	};

	memcpy( 
		kalman->transition_matrix->data.fl, 
		transformMatrix, 
		sizeof(transformMatrix) 
	);

	// 把数组中除了行数与列数相等以外的所有元素的值都设置为0，行数与列数相等的元素的值都设置为1
	cvSetIdentity( 
		kalman->measurement_matrix,
		cvRealScalar(1) 
	);  
	cvSetIdentity(
		kalman->process_noise_cov,
		cvRealScalar(1e-5)
	);  
	cvSetIdentity(
		kalman->measurement_noise_cov,
		cvRealScalar(1e-1)
	);  
	cvSetIdentity(
		kalman->error_cov_post,
		cvRealScalar(1)
	);

	// 初始化后的随机态卡尔曼滤波 
	cvRandArr(
		&amp;rng,
		kalman->state_post,
		CV_RAND_UNI,
		cvRealScalar(0),
		cvRealScalar(windowHeight>windowWidth?windowWidth:windowHeight)
	);
}
CvKalmanPoint::~CvKalmanPoint(void)
{

}

CvPoint CvKalmanPoint::cvKalmanPositionPrediction(void)
{
	// 卡尔曼预测
    const CvMat* prediction = cvKalmanPredict(kalman,0);  
    CvPoint predict_pt=cvPoint((int)prediction->data.fl[0],(int)prediction->data.fl[1]);  

    // 测量校准  
    measurement->data.fl[0] = (float)mousePosition.x;  
    measurement->data.fl[1] = (float)mousePosition.y;  

    // 更新  
    cvKalmanCorrect(kalman, measurement);       

	//printf("预测位置:(%d,%d)\n",predict_pt.x,predict_pt.y);
	//printf("实际位置:(%d,%d)\n",mousePosition.x,mousePosition.y);

	return predict_pt;
}

使用方法：

1
2
3
4
5
6

CvPoint measure;
……
CvKalmanPoint kalman_point;
kalman_point.mousePosition.x = measure.x;
kalman_point.mousePosition.y = measure.y;
measure = kalman_point.cvKalmanPositionPrediction();
0%