编程语言
306
0 卡尔曼OPENCV 预测鼠标位置
卡尔曼滤波不要求信号和噪声都是平稳过程的假设条件。对于每个时刻的系统扰动和观测误差（即噪声），只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误差为最小的真实信号的估计值。 因此，自从卡尔曼滤波理论问世以来，在通信系统、电力系统、航空航天、环境污染控制、工业控制、雷达信号处理等许多部门都得到了应用，取得了许多成功应用的成果。卡尔曼滤波器会对含有噪声的输入数据流（比如计算机视觉中的视频输入）进行递归操作，并产生底层系统状态（比如视频中的位置）在统计意义上的最优估计。
卡尔曼滤波算法分为两个阶段：
预测阶段：卡尔曼滤波器使用由当前点计算的协方差来估计目标的新位置；
更新阶段：卡尔曼滤波器记录目标的位置，并为下一次循环计算修正协方差。
第一版
#include <cv.h> #include <cxcore.h> #include <highgui.h> #include <cmath> #include <vector> #include <iostream> using namespace std; const int winHeight=600; const int winWidth=800; CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1); //mouse event callback void mouseEvent(int event, int x, int y, int flags, void *param ) { if (event==CV_EVENT_MOUSEMOVE) { mousePosition=cvPoint(x,y); } } int main (void) { //1.kalman filter setup const int stateNum=4; const int measureNum=2; CvKalman* kalman = cvCreateKalman( stateNum, measureNum, 0 );//state(x,y,detaX,detaY) CvMat* process_noise = cvCreateMat( stateNum, 1, CV_32FC1 ); CvMat* measurement = cvCreateMat( measureNum, 1, CV_32FC1 );//measurement(x,y) CvRNG rng = cvRNG(-1); float A[stateNum][stateNum] ={//transition matrix 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1 }; memcpy( kalman->transition_matrix->data.fl,A,sizeof(A)); 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)); //initialize post state of kalman filter at random cvRandArr(&rng,kalman->state_post,CV_RAND_UNI,cvRealScalar(0),cvRealScalar(winHeight>winWidth?winWidth:winHeight)); CvFont font; cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1); cvNamedWindow("kalman"); cvSetMouseCallback("kalman",mouseEvent); IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3); while (1){ //2.kalman prediction const CvMat* prediction=cvKalmanPredict(kalman,0); CvPoint predict_pt=cvPoint((int)prediction->data.fl[0],(int)prediction->data.fl[1]); //3.update measurement measurement->data.fl[0]=(float)mousePosition.x; measurement->data.fl[1]=(float)mousePosition.y; //4.update cvKalmanCorrect( kalman, measurement ); //draw cvSet(img,cvScalar(255,255,255,0)); cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);//current position with red char buf[256]; sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y); cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0)); sprintf_s(buf,256,"current position :(%3d,%3d)",mousePosition.x,mousePosition.y); cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0)); cvShowImage("kalman", img); int key=cvWaitKey(3); if (key==27){//esc break; } } cvReleaseImage(&img); cvReleaseKalman(&kalman); return 0; }
第二版程序
#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); //mouse event callback 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; //1.kalman filter setup const int stateNum = 4; //状态值4×1向量(x,y,△x,△y) const int measureNum = 2; //测量值2×1向量(x,y) 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); //转移矩阵A setIdentity(KF.measurementMatrix); //测量矩阵H setIdentity(KF.processNoiseCov, Scalar::all(1e-5)); //系统噪声方差矩阵Q setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1)); //测量噪声方差矩阵R setIdentity(KF.errorCovPost, Scalar::all(1)); //后验错误估计协方差矩阵P rng.fill(KF.statePost, RNG::UNIFORM, 0, winHeight>winWidth ? winWidth : winHeight); //初始状态值x(0) Mat measurement = Mat::zeros(measureNum, 1, CV_32F); //初始测量值x'(0)，因为后面要更新这个值，所以必须先定义 namedWindow("kalman"); setMouseCallback("kalman", mouseEvent); Mat image(winHeight, winWidth, CV_8UC3, Scalar(0)); while (1) { //2.kalman prediction Mat prediction = KF.predict(); Point predict_pt = Point(prediction.at<float>(0), prediction.at<float>(1)); //预测值(x',y') //3.update measurement measurement.at<float>(0) = (float)mousePosition.x; measurement.at<float>(1) = (float)mousePosition.y; //4.update KF.correct(measurement); //draw image.setTo(Scalar(255, 255, 255, 0)); circle(image, predict_pt, 5, Scalar(0, 255, 0), 3); //predicted point with green circle(image, mousePosition, 5, Scalar(255, 0, 0), 3); //current position with red 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){//esc break; } } }
1 OPENCV自带样例
//状态坐标白色
drawCross(statePt, Scalar(255, 255, 255), 3);
//测量坐标蓝色
drawCross(measPt, Scalar(0, 0, 255), 3);
//预测坐标绿色
drawCross(predictPt, Scalar(0, 255, 0), 3);
#include "opencv2/video/tracking.hpp" #include "opencv2/highgui/highgui.hpp" #include <stdio.h> using namespace cv; static inline Point calcPoint(Point2f center, double R, double angle) { return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R; } int main2(int, char**) { /* 使用kalma步骤一 下面语句到for前都是kalman的初始化过程，一般在使用kalman这个类时需要初始化的值有： 转移矩阵，测量矩阵，过程噪声协方差，测量噪声协方差，后验错误协方差矩阵， 前一状态校正后的值，当前观察值 */ Mat img(500, 500, CV_8UC3); KalmanFilter KF(2, 1, 0); Mat state(2, 1, CV_32F); /* (phi, delta_phi) */ 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));//产生均值为0，标准差为0.1的二维高斯列向量 KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 1, 0, 1);//转移矩阵为[1,1;0,1] //函数setIdentity是给参数矩阵对角线赋相同值，默认对角线值值为1 setIdentity(KF.measurementMatrix); setIdentity(KF.processNoiseCov, Scalar::all(1e-5));//系统过程噪声方差矩阵 setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));//测量过程噪声方差矩阵 setIdentity(KF.errorCovPost, Scalar::all(1));//后验错误估计协方差矩阵 //statePost为校正状态，其本质就是前一时刻的状态 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; //state中存放起始角，state为初始状态 double stateAngle = state.at<float>(0); Point statePt = calcPoint(center, R, stateAngle); /* 使用kalma步骤二 调用kalman这个类的predict方法得到状态的预测值矩阵 */ Mat prediction = KF.predict(); //用kalman预测的是角度 double predictAngle = prediction.at<float>(0); Point predictPt = calcPoint(center, R, predictAngle); randn(measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0))); // generate measurement //带噪声的测量 measurement += KF.measurementMatrix*state; double measAngle = measurement.at<float>(0); Point measPt = calcPoint(center, R, measAngle); // plot points //这个define语句是画2条线段(线长很短)，其实就是画一个“X”叉符号 #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); /* 使用kalma步骤三 调用kalman这个类的correct方法得到加入观察值校正后的状态变量值矩阵 */ 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
白色真实位置
蓝色观测位置
绿色实际位置
版本一
//#include <stdafx.h> #include <cv.h> #include <highgui.h> #include <stdio.h> int main() { cvNamedWindow("Kalman", 1); CvRandState random;//创建随机 cvRandInit(&random, 0, 1, -1, CV_RAND_NORMAL); IplImage * image = cvCreateImage(cvSize(600, 450), 8, 3); CvKalman * kalman = cvCreateKalman(4, 2, 0);//状态变量4维，x、y坐标和在x、y方向上的速度，测量变量2维，x、y坐标 CvMat * xK = cvCreateMat(4, 1, CV_32FC1);//初始化状态变量，坐标为（40,40），x、y方向初速度分别为10、10 xK->data.fl[0] = 40.; xK->data.fl[1] = 40; xK->data.fl[2] = 10; xK->data.fl[3] = 10; const float F[] = { 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1 };//初始化传递矩阵 [1 0 1 0] // [0 1 0 1] // [0 0 1 0] // [0 0 0 1] memcpy(kalman->transition_matrix->data.fl, F, sizeof(F)); CvMat * wK = cvCreateMat(4, 1, CV_32FC1);//过程噪声 cvZero(wK); CvMat * zK = cvCreateMat(2, 1, CV_32FC1);//测量矩阵2维，x、y坐标 cvZero(zK); CvMat * vK = cvCreateMat(2, 1, CV_32FC1);//测量噪声 cvZero(vK); cvSetIdentity(kalman->measurement_matrix, cvScalarAll(1));//初始化测量矩阵H=[1 0 0 0] // [0 1 0 0] cvSetIdentity(kalman->process_noise_cov, cvScalarAll(1e-1));/*过程噪声____设置适当数值， 增大目标运动的随机性， 但若设置的很大，则系统不能收敛， 即速度越来越快*/ cvSetIdentity(kalman->measurement_noise_cov, cvScalarAll(10));/*观测噪声____故意将观测噪声设置得很大， 使之测量结果和预测结果同样存在误差*/ cvSetIdentity(kalman->error_cov_post, cvRealScalar(1));/*后验误差协方差*/ cvRand(&random, kalman->state_post); CvMat * mK = cvCreateMat(1, 1, CV_32FC1); //反弹时外加的随机化矩阵 while (1){ cvZero(image); cvRectangle(image, cvPoint(30, 30), cvPoint(570, 420), CV_RGB(255, 255, 255), 2);//绘制目标弹球的“撞击壁” const CvMat * yK = cvKalmanPredict(kalman, 0);//计算预测位置 cvRandSetRange(&random, 0, sqrt(kalman->measurement_noise_cov->data.fl[0]), 0); cvRand(&random, vK);//设置随机的测量误差 cvMatMulAdd(kalman->measurement_matrix, xK, vK, zK);//zK=H*xK+vK cvCircle(image, cvPoint(cvRound(CV_MAT_ELEM(*xK, float, 0, 0)), cvRound(CV_MAT_ELEM(*xK, float, 1, 0))), 4, CV_RGB(255, 255, 255), 2);//白圈，真实位置 cvCircle(image, cvPoint(cvRound(CV_MAT_ELEM(*yK, float, 0, 0)), cvRound(CV_MAT_ELEM(*yK, float, 1, 0))), 4, CV_RGB(0, 255, 0), 2);//绿圈，预估位置 cvCircle(image, cvPoint(cvRound(CV_MAT_ELEM(*zK, float, 0, 0)), cvRound(CV_MAT_ELEM(*zK, float, 1, 0))), 4, CV_RGB(0, 0, 255), 2);//蓝圈，观测位置 cvRandSetRange(&random, 0, sqrt(kalman->process_noise_cov->data.fl[0]), 0); cvRand(&random, wK);//设置随机的过程误差 cvMatMulAdd(kalman->transition_matrix, xK, wK, xK);//xK=F*xK+wK if (cvRound(CV_MAT_ELEM(*xK, float, 0, 0))<30){ //当撞击到反弹壁时，对应轴方向取反外加随机化 cvRandSetRange(&random, 0, sqrt(1e-1), 0); cvRand(&random, mK); xK->data.fl[2] = 10 + CV_MAT_ELEM(*mK, float, 0, 0); } if (cvRound(CV_MAT_ELEM(*xK, float, 0, 0))>570){ cvRandSetRange(&random, 0, sqrt(1e-2), 0); cvRand(&random, mK); xK->data.fl[2] = -(10 + CV_MAT_ELEM(*mK, float, 0, 0)); } if (cvRound(CV_MAT_ELEM(*xK, float, 1, 0))<30){ cvRandSetRange(&random, 0, sqrt(1e-1), 0); cvRand(&random, mK); xK->data.fl[3] = 10 + CV_MAT_ELEM(*mK, float, 0, 0); } if (cvRound(CV_MAT_ELEM(*xK, float, 1, 0))>420){ cvRandSetRange(&random, 0, sqrt(1e-3), 0); cvRand(&random, mK); xK->data.fl[3] = -(10 + CV_MAT_ELEM(*mK, float, 0, 0)); } printf("%f_____%f\n", xK->data.fl[2], xK->data.fl[3]); cvShowImage("Kalman", image); cvKalmanCorrect(kalman, zK); if (cvWaitKey(100) == 'e'){ break; } } cvReleaseImage(&image);/*释放图像*/ cvDestroyAllWindows(); }
本版二
#include "opencv2/video/tracking.hpp" #include "opencv2/highgui/highgui.hpp" #include <stdio.h> using namespace cv; static inline Point calcPoint(Point2f center, double R, double angle) { return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R; } int main2(int, char**) { /* 使用kalma步骤一 下面语句到for前都是kalman的初始化过程，一般在使用kalman这个类时需要初始化的值有： 转移矩阵，测量矩阵，过程噪声协方差，测量噪声协方差，后验错误协方差矩阵， 前一状态校正后的值，当前观察值 */ Mat img(500, 500, CV_8UC3); KalmanFilter KF(2, 1, 0); Mat state(2, 1, CV_32F); /* (phi, delta_phi) */ 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));//产生均值为0，标准差为0.1的二维高斯列向量 KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 1, 0, 1);//转移矩阵为[1,1;0,1] //函数setIdentity是给参数矩阵对角线赋相同值，默认对角线值值为1 setIdentity(KF.measurementMatrix); setIdentity(KF.processNoiseCov, Scalar::all(1e-5));//系统过程噪声方差矩阵 setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));//测量过程噪声方差矩阵 setIdentity(KF.errorCovPost, Scalar::all(1));//后验错误估计协方差矩阵 //statePost为校正状态，其本质就是前一时刻的状态 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; //state中存放起始角，state为初始状态 double stateAngle = state.at<float>(0); Point statePt = calcPoint(center, R, stateAngle); /* 使用kalma步骤二 调用kalman这个类的predict方法得到状态的预测值矩阵 */ Mat prediction = KF.predict(); //用kalman预测的是角度 double predictAngle = prediction.at<float>(0); Point predictPt = calcPoint(center, R, predictAngle); randn(measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0))); // generate measurement //带噪声的测量 measurement += KF.measurementMatrix*state; double measAngle = measurement.at<float>(0); Point measPt = calcPoint(center, R, measAngle); // plot points //这个define语句是画2条线段(线长很短)，其实就是画一个“X”叉符号 #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); /* 使用kalma步骤三 调用kalman这个类的correct方法得到加入观察值校正后的状态变量值矩阵 */ 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; }
