本文将简要回顾一下卡尔曼滤波理论,然后详细介绍如何在OpenCV中使用卡尔曼滤波进行跟踪,最后给两个程序实例。
1. 卡尔曼滤波理论回顾
对于一个动态系统,我们首先定义一组状态空间方程
状态方程:
测量方程:
xk是状态向量,zk是测量向量,Ak是状态转移矩阵,uk是控制向量,Bk是控制矩阵,wk是系统误差(噪声),Hk是测量矩阵,vk是测量误差(噪声)。wk和vk都是高斯噪声,即
整个卡尔曼滤波的过程就是个递推计算的过程,不断的“预测——更新——预测——更新……”
预测
预测状态值:
预测最小均方误差:
更新
测量误差:
测量协方差:
最优卡尔曼增益:
修正状态值:
修正最小均方误差:
2.OpenCV中的KalmanFilter详解
OpenCV中有两个版本的卡尔曼滤波方法KalmanFilter(C++)和CvKalman(C),用法差不太多,这里只介绍KalmanFilter。
C++版本中将KalmanFilter封装到一个类中,其结构如下所示:
- class CV_EXPORTS_W KalmanFilter
- {
- public:
- CV_WRAP KalmanFilter();
- CV_WRAP KalmanFilter(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);
- void init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);
-
- CV_WRAP const Mat& predict(const Mat& control=Mat());
- CV_WRAP const Mat& correct(const Mat& measurement);
-
- Mat statePre;
- Mat statePost;
- Mat transitionMatrix;
- Mat controlMatrix;
- Mat measurementMatrix;
- Mat processNoiseCov;
- Mat measurementNoiseCov;
- Mat errorCovPre;
- Mat gain;
- Mat errorCovPost;
-
-
- Mat temp1;
- Mat temp2;
- Mat temp3;
- Mat temp4;
- Mat temp5;
- };
-
- enum
- {
- OPTFLOW_USE_INITIAL_FLOW = CV_LKFLOW_INITIAL_GUESSES,
- OPTFLOW_LK_GET_MIN_EIGENVALS = CV_LKFLOW_GET_MIN_EIGENVALS,
- OPTFLOW_FARNEBACK_GAUSSIAN = 256
- };
函数原型见:…..\OpenCV2\sources\modules\ocl\src\kalman.cpp
只有四个方法: 构造KF对象KalmanFilter(DP,MP,CP)、初始化KF对象init(DP,MP,CP)、预测predict( )、更新correct( )。除非你要重新构造KF对象,否则用不到init( )。
KalmanFilter(DP,MP,CP)和init( )就是赋值,没什么好说的。
注意:KalmanFilter结构体中并没有测量值,测量值需要自己定义,而且一定要定义,因为后面要用。
编程步骤
step1:定义KalmanFilter类并初始化
//构造KF对象
KalmanFilter KF(DP, MP, 0);
//初始化相关参数
KF.transitionMatrix 转移矩阵 A
KF.measurementMatrix 测量矩阵 H
KF.processNoiseCov 过程噪声 Q
KF.measurementNoiseCov 测量噪声 R
KF.errorCovPost 最小均方误差 P
KF.statePost 系统初始状态 x(0)
Mat measurement 定义初始测量值 z(0)
step2:预测
KF.predict( ) //返回的是下一时刻的状态值KF.statePost (k+1)
step3:更新
更新measurement; //注意measurement不能通过观测方程进行计算得到,要自己定义!
更新KF KF.correct(measurement)
最终的结果应该是更新后的statePost.
相关参数的确定
对于系统状态方程,简记为Y=AX+B,X和Y是表示系统状态的列向量,A是转移矩阵,B是其他项。
状态值(向量)只要能表示系统的状态即可,状态值的维数决定了转移矩阵A的维数,比如X和Y是N×1的,则A是N×N的。
A的确定跟X有关,只要保证方程中不相干项的系数为0即可,看下面例子
X和Y是二维的,
X和Y是三维的,
X和Y是三维的,但c和△ c是相关项
上面的1也可以是其他值。
下面对predict( ) 和correct( )函数介绍下,可以不用看,不影响编程。
- CV_EXPORTS const oclMat& KalmanFilter::predict(const oclMat& control)
- {
- gemm(transitionMatrix, statePost, 1, oclMat(), 0, statePre);
- oclMat temp;
-
- if(control.data)
- gemm(controlMatrix, control, 1, statePre, 1, statePre);
- gemm(transitionMatrix, errorCovPost, 1, oclMat(), 0, temp1);
- gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);
- statePre.copyTo(statePost);
- return statePre;
- }
gemm( )是矩阵的广义乘法
void gemm(const GpuMat& src1, constGpuMat& src2, double alpha, const GpuMat& src3, double beta,GpuMat& dst, int flags=0, Stream& stream=Stream::Null())
dst = alpha · src1 · src2 +beta· src3
上面,oclMat()其实是uk,只不过默认为0,所以没赋值。整个过程就计算了x'和P’。(用x'代表x的预测值,用P'代表P的预测值)。GEMM_2_T表示对第2个参数转置。
x’(k)=1·A·x(k-1)
如果B非空, x'(k) = 1·B·u + 1·x'(k-1)
temp1 = 1·A·P(k-1) + 0·u(k)
P’(k) = 1· temp1·AT + 1· Qk= A·P(k-1)·AT + 1· Qk
可见,和第一部分的理论介绍完全一致。
- CV_EXPORTS const oclMat& KalmanFilter::correct(const oclMat& measurement)
- {
- CV_Assert(measurement.empty() == false);
- gemm(measurementMatrix, errorCovPre, 1, oclMat(), 0, temp2);
- gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);
- Mat temp;
- solve(Mat(temp3), Mat(temp2), temp, DECOMP_SVD);
- temp4.upload(temp);
- gain = temp4.t();
- gemm(measurementMatrix, statePre, -1, measurement, 1, temp5);
- gemm(gain, temp5, 1, statePre, 1, statePost);
- gemm(gain, temp2, -1, errorCovPre, 1, errorCovPost);
- return statePost;
- }
bool solve(InputArray src1, InputArray src2, OutputArray dst, int flags=DECOMP_LU)
求解线型最小二乘估计
temp2 = 1· H·P’ + 0·u(k)
temp3 = 1· temp2·HT + 1·R = H·P’·HT+ 1· R 也就是上面的Sk
temp = argmin||tem2- temp3||
K=temp
temp5 = -1· H·x’ + 1·zk 就是上面的y’。
x = 1·K·temp5 + 1·x’ = KT·y’ +x’
P =-1·K·temp2 + 1·P’ = -K·H·P’+P’ = (I- K·H) P’
也和第一部分的理论完全一致。
通过深入函数内部,学到了两个实用的函数哦。矩阵广义乘法gemm( )、最小二乘估计solve( )
补充:
1)以例2为例,为什么状态值一般都设置成(x,y,△x,△y)?我们不妨设置成(x,y,△x),对应的转移矩阵也改成3×3的。可以看到仍能跟上,不过在x方向跟踪速度快,在y方向跟踪速度慢。进一步设置成(x,y)和2×2的转移矩阵,程序的跟踪速度简直是龟速。所以,简单理解,△x和△y严重影响对应方向上的跟踪速度。
3.实例
例1 OpenCV自带的示例程序
- #include "opencv2/video/tracking.hpp"
- #include "opencv2/highgui/highgui.hpp"
- #include <iostream>
- #include <stdio.h>
- using namespace std;
- using namespace cv;
-
-
- static inline Point calcPoint(Point2f center, double R, double angle)
- {
- return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R;
- }
-
- static void help()
- {
- printf( "\nExamle of c calls to OpenCV's Kalman filter.\n"
- " Tracking of rotating point.\n"
- " Rotation speed is constant.\n"
- " Both state and measurements vectors are 1D (a point angle),\n"
- " Measurement is the real point angle + gaussian noise.\n"
- " The real and the estimated points are connected with yellow line segment,\n"
- " the real and the measured points are connected with red line segment.\n"
- " (if Kalman filter works correctly,\n"
- " the yellow segment should be shorter than the red one).\n"
- "\n"
- " Pressing any key (except ESC) will reset the tracking with a different speed.\n"
- " Pressing ESC will stop the program.\n"
- );
- }
-
- int main(int, char**)
- {
- help();
- Mat img(500, 500, CV_8UC3);
- KalmanFilter KF(2, 1, 0);
- Mat state(2, 1, CV_32F);
- Mat processNoise(2, 1, CV_32F);
- Mat measurement = Mat::zeros(1, 1, CV_32F);
- char code = (char)-1;
-
- for(;;)
- {
-
- randn( state, Scalar::all(0), Scalar::all(0.1) );
- KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 1, 0, 1);
-
-
-
- setIdentity(KF.measurementMatrix);
- setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
- setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
- setIdentity(KF.errorCovPost, Scalar::all(1));
-
- randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));
-
- for(;;)
- {
- Point2f center(img.cols*0.5f, img.rows*0.5f);
- float R = img.cols/3.f;
- double stateAngle = state.at<float>(0);
- Point statePt = calcPoint(center, R, stateAngle);
-
-
- Mat prediction = KF.predict();
- double predictAngle = prediction.at<float>(0);
- Point predictPt = calcPoint(center, R, predictAngle);
-
-
-
-
- randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0)));
-
-
- measurement += KF.measurementMatrix*state;
-
- double measAngle = measurement.at<float>(0);
- Point measPt = calcPoint(center, R, measAngle);
-
-
-
- #define drawCross( center, color, d ) \
- line( img, Point( center.x - d, center.y - d ), \
- Point( center.x + d, center.y + d ), color, 1, CV_AA, 0); \
- line( img, Point( center.x + d, center.y - d ), \
- Point( center.x - d, center.y + d ), color, 1, CV_AA, 0 )
-
- img = Scalar::all(0);
- drawCross( statePt, Scalar(255,255,255), 3 );
- drawCross( measPt, Scalar(0,0,255), 3 );
- drawCross( predictPt, Scalar(0,255,0), 3 );
- line( img, statePt, measPt, Scalar(0,0,255), 3, CV_AA, 0 );
- line( img, statePt, predictPt, Scalar(0,255,255), 3, CV_AA, 0 );
-
-
-
- if(theRNG().uniform(0,4) != 0)
- KF.correct(measurement);
-
-
- randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));
- state = KF.transitionMatrix*state + processNoise;
-
- imshow( "Kalman", img );
- code = (char)waitKey(100);
-
- if( code > 0 )
- break;
- }
- if( code == 27 || code == 'q' || code == 'Q' )
- break;
- }
-
- return 0;
- }
程序结果
例2 跟踪鼠标位置
在我介绍粒子滤波的博文“学习Opencv2——粒子滤波Condensation算法”里,有个例3,是跟踪鼠标位置。现在我们用卡尔曼滤波来实现。
- #include "opencv2/video/tracking.hpp"
- #include "opencv2/highgui/highgui.hpp"
- #include <stdio.h>
- using namespace cv;
- using namespace std;
-
- const int winHeight=600;
- const int winWidth=800;
-
-
- Point mousePosition= Point(winWidth>>1,winHeight>>1);
-
-
- void mouseEvent(int event, int x, int y, int flags, void *param )
- {
- if (event==CV_EVENT_MOUSEMOVE) {
- mousePosition = Point(x,y);
- }
- }
-
- int main (void)
- {
- RNG rng;
-
- const int stateNum=4;
- const int measureNum=2;
- KalmanFilter KF(stateNum, measureNum, 0);
-
- KF.transitionMatrix = *(Mat_<float>(4, 4) <<1,0,1,0,0,1,0,1,0,0,1,0,0,0,0,1);
- setIdentity(KF.measurementMatrix);
- setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
- setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
- setIdentity(KF.errorCovPost, Scalar::all(1));
- rng.fill(KF.statePost,RNG::UNIFORM,0,winHeight>winWidth?winWidth:winHeight);
- Mat measurement = Mat::zeros(measureNum, 1, CV_32F);
-
- namedWindow("kalman");
- setMouseCallback("kalman",mouseEvent);
-
- Mat image(winHeight,winWidth,CV_8UC3,Scalar(0));
-
- while (1)
- {
-
- Mat prediction = KF.predict();
- Point predict_pt = Point(prediction.at<float>(0),prediction.at<float>(1) );
-
-
- measurement.at<float>(0) = (float)mousePosition.x;
- measurement.at<float>(1) = (float)mousePosition.y;
-
-
- KF.correct(measurement);
-
-
- image.setTo(Scalar(255,255,255,0));
- circle(image,predict_pt,5,Scalar(0,255,0),3);
- circle(image,mousePosition,5,Scalar(255,0,0),3);
-
- char buf[256];
- sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);
- putText(image,buf,Point(10,30),CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,Scalar(0,0,0),1,8);
- sprintf_s(buf,256,"current position :(%3d,%3d)",mousePosition.x,mousePosition.y);
- putText(image,buf,cvPoint(10,60),CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,Scalar(0,0,0),1,8);
-
- imshow("kalman", image);
- int key=waitKey(3);
- if (key==27){
- break;
- }
- }
- }
结果
例3
- #include "opencv2/video/tracking.hpp"
- #include <opencv2/legacy/legacy.hpp> //#include "cvAux.h"
- #include <opencv2/highgui/highgui.hpp>
- #include <opencv2/core/core.hpp>
- #include <stdio.h>
-
- using namespace cv;
- using namespace std;
-
- int main( )
- {
- float A[10][3] =
- {
- 10, 50, 15.6,
- 12, 49, 16,
- 11, 52, 15.8,
- 13, 52.2, 15.8,
- 12.9, 50, 17,
- 14, 48, 16.6,
- 13.7, 49, 16.5,
- 13.6, 47.8, 16.4,
- 12.3, 46, 15.9,
- 13.1, 45, 16.2
- };
-
- const int stateNum=3;
- const int measureNum=3;
- KalmanFilter KF(stateNum, measureNum, 0);
- KF.transitionMatrix = *(Mat_<float>(3, 3) <<1,0,0,0,1,0,0,0,1);
- setIdentity(KF.measurementMatrix);
- setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
- setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
- setIdentity(KF.errorCovPost, Scalar::all(1));
- Mat measurement = Mat::zeros(measureNum, 1, CV_32F);
-
-
- KF.statePost = *(Mat_<float>(3, 1) <<A[0][0],A[0][1],A[0][2]);
- cout<<"state0="<<KF.statePost<<endl;
-
- for(int i=1;i<=9;i++)
- {
-
- Mat prediction = KF.predict();
-
- measurement.at<float>(0) = (float)A[i][0];
- measurement.at<float>(1) = (float)A[i][1];
- measurement.at<float>(2) = (float)A[i][2];
-
- KF.correct(measurement);
-
- cout<<"predict ="<<"\t"<<prediction.at<float>(0)<<"\t"<<prediction.at<float>(1)<<"\t"<<prediction.at<float>(2)<<endl;
- cout<<"measurement="<<"\t"<<measurement.at<float>(0)<<"\t"<<measurement.at<float>(1)<<"\t"<<measurement.at<float>(2)<<endl;
- cout<<"correct ="<<"\t"<<KF.statePost.at<float>(0)<<"\t"<<KF.statePost.at<float>(1)<<"\t"<<KF.statePost.at<float>(2)<<endl;
- }
- system("pause");
- }
结果如下
这里预测值和上一个状态值一样,原因是转移矩阵A是单位阵,如果改成非单位阵,结果就不一样了。
原文:https://i-blog.csdnimg.cn/blog_migrate/f520b5b02f5acdca25777a718ecc67a8.png