0 卡尔曼OPENCV 预测鼠标位置
1卡尔曼滤波不要求信号和噪声都是平稳过程的假设条件。对于每个时刻的系统扰动和观测误差（即噪声），只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误差为最小的真实信号的估计值。 2 3因此，自从卡尔曼滤波理论问世以来，在通信系统、电力系统、航空航天、环境污染控制、工业控制、雷达信号处理等许多部门都得到了应用，取得了许多成功应用的成果。卡尔曼滤波器会对含有噪声的输入数据流（比如计算机视觉中的视频输入）进行递归操作，并产生底层系统状态（比如视频中的位置）在统计意义上的最优估计。
卡尔曼滤波算法分为两个阶段：
预测阶段：卡尔曼滤波器使用由当前点计算的协方差来估计目标的新位置；
更新阶段：卡尔曼滤波器记录目标的位置，并为下一次循环计算修正协方差。
第一版
1#include <cv.h> 2#include <cxcore.h> 3#include <highgui.h> 4 5#include <cmath> 6#include <vector> 7#include <iostream> 8using namespace std; 9 10const int winHeight=600; 11const int winWidth=800; 12 13 14CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1); 15 16//mouse event callback 17void mouseEvent(int event, int x, int y, int flags, void *param ) 18{ 19 if (event==CV_EVENT_MOUSEMOVE) { 20 mousePosition=cvPoint(x,y); 21 } 22} 23 24int main (void) 25{ 26 //1.kalman filter setup 27 const int stateNum=4; 28 const int measureNum=2; 29 CvKalman* kalman = cvCreateKalman( stateNum, measureNum, 0 );//state(x,y,detaX,detaY) 30 CvMat* process_noise = cvCreateMat( stateNum, 1, CV_32FC1 ); 31 CvMat* measurement = cvCreateMat( measureNum, 1, CV_32FC1 );//measurement(x,y) 32 CvRNG rng = cvRNG(-1); 33 float A[stateNum][stateNum] ={//transition matrix 34 1,0,1,0, 35 0,1,0,1, 36 0,0,1,0, 37 0,0,0,1 38 }; 39 40 memcpy( kalman->transition_matrix->data.fl,A,sizeof(A)); 41 cvSetIdentity(kalman->measurement_matrix,cvRealScalar(1) ); 42 cvSetIdentity(kalman->process_noise_cov,cvRealScalar(1e-5)); 43 cvSetIdentity(kalman->measurement_noise_cov,cvRealScalar(1e-1)); 44 cvSetIdentity(kalman->error_cov_post,cvRealScalar(1)); 45 //initialize post state of kalman filter at random 46 cvRandArr(&rng,kalman->state_post,CV_RAND_UNI,cvRealScalar(0),cvRealScalar(winHeight>winWidth?winWidth:winHeight)); 47 48 CvFont font; 49 cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1); 50 51 cvNamedWindow("kalman"); 52 cvSetMouseCallback("kalman",mouseEvent); 53 IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3); 54 while (1){ 55 //2.kalman prediction 56 const CvMat* prediction=cvKalmanPredict(kalman,0); 57 CvPoint predict_pt=cvPoint((int)prediction->data.fl[0],(int)prediction->data.fl[1]); 58 59 //3.update measurement 60 measurement->data.fl[0]=(float)mousePosition.x; 61 measurement->data.fl[1]=(float)mousePosition.y; 62 63 //4.update 64 cvKalmanCorrect( kalman, measurement ); 65 66 //draw 67 cvSet(img,cvScalar(255,255,255,0)); 68 cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green 69 cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);//current position with red 70 char buf[256]; 71 sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y); 72 cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0)); 73 sprintf_s(buf,256,"current position :(%3d,%3d)",mousePosition.x,mousePosition.y); 74 cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0)); 75 76 cvShowImage("kalman", img); 77 int key=cvWaitKey(3); 78 if (key==27){//esc 79 break; 80 } 81 } 82 83 cvReleaseImage(&img); 84 cvReleaseKalman(&kalman); 85 return 0; 86}
第二版程序
1#include "opencv2/video/tracking.hpp" 2#include "opencv2/highgui/highgui.hpp" 3#include <stdio.h> 4using namespace cv; 5using namespace std; 6 7const int winHeight = 600; 8const int winWidth = 800; 9 10 11Point mousePosition = Point(winWidth >> 1, winHeight >> 1); 12 13//mouse event callback 14void mouseEvent(int event, int x, int y, int flags, void *param) 15{ 16 if (event == CV_EVENT_MOUSEMOVE) { 17 mousePosition = Point(x, y); 18 } 19} 20 21int main(void) 22{ 23 RNG rng; 24 //1.kalman filter setup 25 const int stateNum = 4; //状态值4×1向量(x,y,△x,△y) 26 const int measureNum = 2; //测量值2×1向量(x,y) 27 KalmanFilter KF(stateNum, measureNum, 0); 28 29 KF.transitionMatrix = *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1); //转移矩阵A 30 setIdentity(KF.measurementMatrix); //测量矩阵H 31 setIdentity(KF.processNoiseCov, Scalar::all(1e-5)); //系统噪声方差矩阵Q 32 setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1)); //测量噪声方差矩阵R 33 setIdentity(KF.errorCovPost, Scalar::all(1)); //后验错误估计协方差矩阵P 34 rng.fill(KF.statePost, RNG::UNIFORM, 0, winHeight>winWidth ? winWidth : winHeight); //初始状态值x(0) 35 Mat measurement = Mat::zeros(measureNum, 1, CV_32F); //初始测量值x'(0)，因为后面要更新这个值，所以必须先定义 36 37 namedWindow("kalman"); 38 setMouseCallback("kalman", mouseEvent); 39 40 Mat image(winHeight, winWidth, CV_8UC3, Scalar(0)); 41 42 while (1) 43 { 44 //2.kalman prediction 45 Mat prediction = KF.predict(); 46 Point predict_pt = Point(prediction.at<float>(0), prediction.at<float>(1)); //预测值(x',y') 47 48 //3.update measurement 49 measurement.at<float>(0) = (float)mousePosition.x; 50 measurement.at<float>(1) = (float)mousePosition.y; 51 52 //4.update 53 KF.correct(measurement); 54 55 //draw 56 image.setTo(Scalar(255, 255, 255, 0)); 57 circle(image, predict_pt, 5, Scalar(0, 255, 0), 3); //predicted point with green 58 circle(image, mousePosition, 5, Scalar(255, 0, 0), 3); //current position with red 59 60 char buf[256]; 61 sprintf_s(buf, 256, "predicted position:(%3d,%3d)", predict_pt.x, predict_pt.y); 62 putText(image, buf, Point(10, 30), CV_FONT_HERSHEY_SCRIPT_COMPLEX, 1, Scalar(0, 0, 0), 1, 8); 63 sprintf_s(buf, 256, "current position :(%3d,%3d)", mousePosition.x, mousePosition.y); 64 putText(image, buf, cvPoint(10, 60), CV_FONT_HERSHEY_SCRIPT_COMPLEX, 1, Scalar(0, 0, 0), 1, 8); 65 66 imshow("kalman", image); 67 int key = waitKey(3); 68 if (key == 27){//esc 69 break; 70 } 71 } 72}
1 OPENCV自带样例
//状态坐标白色
drawCross(statePt, Scalar(255, 255, 255), 3);
//测量坐标蓝色
drawCross(measPt, Scalar(0, 0, 255), 3);
//预测坐标绿色
drawCross(predictPt, Scalar(0, 255, 0), 3);
1#include "opencv2/video/tracking.hpp" 2#include "opencv2/highgui/highgui.hpp" 3 4#include <stdio.h> 5 6using namespace cv; 7 8static inline Point calcPoint(Point2f center, double R, double angle) 9{ 10 return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R; 11} 12 13 14 15int main2(int, char**) 16{ 17 /* 18 使用kalma步骤一 19 下面语句到for前都是kalman的初始化过程，一般在使用kalman这个类时需要初始化的值有： 20 转移矩阵，测量矩阵，过程噪声协方差，测量噪声协方差，后验错误协方差矩阵， 21 前一状态校正后的值，当前观察值 22 */ 23 24 25 Mat img(500, 500, CV_8UC3); 26 KalmanFilter KF(2, 1, 0); 27 Mat state(2, 1, CV_32F); /* (phi, delta_phi) */ 28 Mat processNoise(2, 1, CV_32F); 29 Mat measurement = Mat::zeros(1, 1, CV_32F); 30 char code = (char)-1; 31 32 for (;;) 33 { 34 randn(state, Scalar::all(0), Scalar::all(0.1));//产生均值为0，标准差为0.1的二维高斯列向量 35 KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 1, 0, 1);//转移矩阵为[1,1;0,1] 36 37 //函数setIdentity是给参数矩阵对角线赋相同值，默认对角线值值为1 38 setIdentity(KF.measurementMatrix); 39 setIdentity(KF.processNoiseCov, Scalar::all(1e-5));//系统过程噪声方差矩阵 40 setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));//测量过程噪声方差矩阵 41 setIdentity(KF.errorCovPost, Scalar::all(1));//后验错误估计协方差矩阵 42 43 //statePost为校正状态，其本质就是前一时刻的状态 44 randn(KF.statePost, Scalar::all(0), Scalar::all(0.1)); 45 46 for (;;) 47 { 48 Point2f center(img.cols*0.5f, img.rows*0.5f); 49 float R = img.cols / 3.f; 50 //state中存放起始角，state为初始状态 51 double stateAngle = state.at<float>(0); 52 Point statePt = calcPoint(center, R, stateAngle); 53 54 55 /* 56 使用kalma步骤二 57 调用kalman这个类的predict方法得到状态的预测值矩阵 58 */ 59 60 61 Mat prediction = KF.predict(); 62 //用kalman预测的是角度 63 double predictAngle = prediction.at<float>(0); 64 Point predictPt = calcPoint(center, R, predictAngle); 65 66 randn(measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0))); 67 68 // generate measurement 69 //带噪声的测量 70 measurement += KF.measurementMatrix*state; 71 72 double measAngle = measurement.at<float>(0); 73 Point measPt = calcPoint(center, R, measAngle); 74 75 // plot points 76 //这个define语句是画2条线段(线长很短)，其实就是画一个“X”叉符号 77 78#define drawCross( center, color, d ) \ 79 line( img, Point( center.x - d, center.y - d ), \ 80 Point( center.x + d, center.y + d ), color, 1, CV_AA, 0); \ 81 line( img, Point( center.x + d, center.y - d ), \ 82 Point( center.x - d, center.y + d ), color, 1, CV_AA, 0 ) 83 84 img = Scalar::all(0); 85 //状态坐标白色 86 drawCross(statePt, Scalar(255, 255, 255), 3); 87 //测量坐标蓝色 88 drawCross(measPt, Scalar(0, 0, 255), 3); 89 //预测坐标绿色 90 drawCross(predictPt, Scalar(0, 255, 0), 3); 91 //真实值和测量值之间用红色线连接起来 92 line(img, statePt, measPt, Scalar(0, 0, 255), 3, CV_AA, 0); 93 //真实值和估计值之间用黄色线连接起来 94 line(img, statePt, predictPt, Scalar(0, 255, 255), 3, CV_AA, 0); 95 96 97 /* 98 使用kalma步骤三 99 调用kalman这个类的correct方法得到加入观察值校正后的状态变量值矩阵 100 */ 101 102 if (theRNG().uniform(0, 4) != 0) 103 KF.correct(measurement); 104 105 randn(processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0)))); 106 //不加噪声的话就是匀速圆周运动，加了点噪声类似匀速圆周运动，因为噪声的原因，运动方向可能会改变 107 state = KF.transitionMatrix*state + processNoise; 108 109 imshow("Kalman", img); 110 code = (char)waitKey(100); 111 112 if (code > 0) 113 break; 114 } 115 if (code == 27 || code == 'q' || code == 'Q') 116 break; 117 } 118 119 return 0; 120}
2
白色真实位置
蓝色观测位置
绿色实际位置
版本一
1//#include <stdafx.h> 2#include <cv.h> 3#include <highgui.h> 4#include <stdio.h> 5 6int main() 7{ 8 cvNamedWindow("Kalman", 1); 9 CvRandState random;//创建随机 10 cvRandInit(&random, 0, 1, -1, CV_RAND_NORMAL); 11 IplImage * image = cvCreateImage(cvSize(600, 450), 8, 3); 12 CvKalman * kalman = cvCreateKalman(4, 2, 0);//状态变量4维，x、y坐标和在x、y方向上的速度，测量变量2维，x、y坐标 13 14 CvMat * xK = cvCreateMat(4, 1, CV_32FC1);//初始化状态变量，坐标为（40,40），x、y方向初速度分别为10、10 15 xK->data.fl[0] = 40.; 16 xK->data.fl[1] = 40; 17 xK->data.fl[2] = 10; 18 xK->data.fl[3] = 10; 19 20 const float F[] = { 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1 };//初始化传递矩阵 [1 0 1 0] 21 // [0 1 0 1] 22 // [0 0 1 0] 23 // [0 0 0 1] 24 memcpy(kalman->transition_matrix->data.fl, F, sizeof(F)); 25 26 27 28 CvMat * wK = cvCreateMat(4, 1, CV_32FC1);//过程噪声 29 cvZero(wK); 30 31 CvMat * zK = cvCreateMat(2, 1, CV_32FC1);//测量矩阵2维，x、y坐标 32 cvZero(zK); 33 34 CvMat * vK = cvCreateMat(2, 1, CV_32FC1);//测量噪声 35 cvZero(vK); 36 37 cvSetIdentity(kalman->measurement_matrix, cvScalarAll(1));//初始化测量矩阵H=[1 0 0 0] 38 // [0 1 0 0] 39 cvSetIdentity(kalman->process_noise_cov, cvScalarAll(1e-1));/*过程噪声____设置适当数值， 40 增大目标运动的随机性， 41 但若设置的很大，则系统不能收敛， 42 即速度越来越快*/ 43 cvSetIdentity(kalman->measurement_noise_cov, cvScalarAll(10));/*观测噪声____故意将观测噪声设置得很大， 44 使之测量结果和预测结果同样存在误差*/ 45 cvSetIdentity(kalman->error_cov_post, cvRealScalar(1));/*后验误差协方差*/ 46 cvRand(&random, kalman->state_post); 47 48 CvMat * mK = cvCreateMat(1, 1, CV_32FC1); //反弹时外加的随机化矩阵 49 50 51 while (1){ 52 cvZero(image); 53 cvRectangle(image, cvPoint(30, 30), cvPoint(570, 420), CV_RGB(255, 255, 255), 2);//绘制目标弹球的“撞击壁” 54 const CvMat * yK = cvKalmanPredict(kalman, 0);//计算预测位置 55 cvRandSetRange(&random, 0, sqrt(kalman->measurement_noise_cov->data.fl[0]), 0); 56 cvRand(&random, vK);//设置随机的测量误差 57 cvMatMulAdd(kalman->measurement_matrix, xK, vK, zK);//zK=H*xK+vK 58 cvCircle(image, cvPoint(cvRound(CV_MAT_ELEM(*xK, float, 0, 0)), cvRound(CV_MAT_ELEM(*xK, float, 1, 0))), 59 4, CV_RGB(255, 255, 255), 2);//白圈，真实位置 60 cvCircle(image, cvPoint(cvRound(CV_MAT_ELEM(*yK, float, 0, 0)), cvRound(CV_MAT_ELEM(*yK, float, 1, 0))), 61 4, CV_RGB(0, 255, 0), 2);//绿圈，预估位置 62 cvCircle(image, cvPoint(cvRound(CV_MAT_ELEM(*zK, float, 0, 0)), cvRound(CV_MAT_ELEM(*zK, float, 1, 0))), 63 4, CV_RGB(0, 0, 255), 2);//蓝圈，观测位置 64 65 cvRandSetRange(&random, 0, sqrt(kalman->process_noise_cov->data.fl[0]), 0); 66 cvRand(&random, wK);//设置随机的过程误差 67 cvMatMulAdd(kalman->transition_matrix, xK, wK, xK);//xK=F*xK+wK 68 69 if (cvRound(CV_MAT_ELEM(*xK, float, 0, 0))<30){ //当撞击到反弹壁时，对应轴方向取反外加随机化 70 cvRandSetRange(&random, 0, sqrt(1e-1), 0); 71 cvRand(&random, mK); 72 xK->data.fl[2] = 10 + CV_MAT_ELEM(*mK, float, 0, 0); 73 } 74 if (cvRound(CV_MAT_ELEM(*xK, float, 0, 0))>570){ 75 cvRandSetRange(&random, 0, sqrt(1e-2), 0); 76 cvRand(&random, mK); 77 xK->data.fl[2] = -(10 + CV_MAT_ELEM(*mK, float, 0, 0)); 78 } 79 if (cvRound(CV_MAT_ELEM(*xK, float, 1, 0))<30){ 80 cvRandSetRange(&random, 0, sqrt(1e-1), 0); 81 cvRand(&random, mK); 82 xK->data.fl[3] = 10 + CV_MAT_ELEM(*mK, float, 0, 0); 83 } 84 if (cvRound(CV_MAT_ELEM(*xK, float, 1, 0))>420){ 85 cvRandSetRange(&random, 0, sqrt(1e-3), 0); 86 cvRand(&random, mK); 87 xK->data.fl[3] = -(10 + CV_MAT_ELEM(*mK, float, 0, 0)); 88 } 89 90 printf("%f_____%f\n", xK->data.fl[2], xK->data.fl[3]); 91 92 93 cvShowImage("Kalman", image); 94 95 cvKalmanCorrect(kalman, zK); 96 97 98 if (cvWaitKey(100) == 'e'){ 99 break; 100 } 101 } 102 103 104 cvReleaseImage(&image);/*释放图像*/ 105 cvDestroyAllWindows(); 106}
本版二
1#include "opencv2/video/tracking.hpp" 2#include "opencv2/highgui/highgui.hpp" 3 4#include <stdio.h> 5 6using namespace cv; 7 8static inline Point calcPoint(Point2f center, double R, double angle) 9{ 10 return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R; 11} 12 13 14 15int main2(int, char**) 16{ 17 /* 18 使用kalma步骤一 19 下面语句到for前都是kalman的初始化过程，一般在使用kalman这个类时需要初始化的值有： 20 转移矩阵，测量矩阵，过程噪声协方差，测量噪声协方差，后验错误协方差矩阵， 21 前一状态校正后的值，当前观察值 22 */ 23 24 25 Mat img(500, 500, CV_8UC3); 26 KalmanFilter KF(2, 1, 0); 27 Mat state(2, 1, CV_32F); /* (phi, delta_phi) */ 28 Mat processNoise(2, 1, CV_32F); 29 Mat measurement = Mat::zeros(1, 1, CV_32F); 30 char code = (char)-1; 31 32 for (;;) 33 { 34 randn(state, Scalar::all(0), Scalar::all(0.1));//产生均值为0，标准差为0.1的二维高斯列向量 35 KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 1, 0, 1);//转移矩阵为[1,1;0,1] 36 37 //函数setIdentity是给参数矩阵对角线赋相同值，默认对角线值值为1 38 setIdentity(KF.measurementMatrix); 39 setIdentity(KF.processNoiseCov, Scalar::all(1e-5));//系统过程噪声方差矩阵 40 setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));//测量过程噪声方差矩阵 41 setIdentity(KF.errorCovPost, Scalar::all(1));//后验错误估计协方差矩阵 42 43 //statePost为校正状态，其本质就是前一时刻的状态 44 randn(KF.statePost, Scalar::all(0), Scalar::all(0.1)); 45 46 for (;;) 47 { 48 Point2f center(img.cols*0.5f, img.rows*0.5f); 49 float R = img.cols / 3.f; 50 //state中存放起始角，state为初始状态 51 double stateAngle = state.at<float>(0); 52 Point statePt = calcPoint(center, R, stateAngle); 53 54 55 /* 56 使用kalma步骤二 57 调用kalman这个类的predict方法得到状态的预测值矩阵 58 */ 59 60 61 Mat prediction = KF.predict(); 62 //用kalman预测的是角度 63 double predictAngle = prediction.at<float>(0); 64 Point predictPt = calcPoint(center, R, predictAngle); 65 66 randn(measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0))); 67 68 // generate measurement 69 //带噪声的测量 70 measurement += KF.measurementMatrix*state; 71 72 double measAngle = measurement.at<float>(0); 73 Point measPt = calcPoint(center, R, measAngle); 74 75 // plot points 76 //这个define语句是画2条线段(线长很短)，其实就是画一个“X”叉符号 77 78#define drawCross( center, color, d ) \ 79 line( img, Point( center.x - d, center.y - d ), \ 80 Point( center.x + d, center.y + d ), color, 1, CV_AA, 0); \ 81 line( img, Point( center.x + d, center.y - d ), \ 82 Point( center.x - d, center.y + d ), color, 1, CV_AA, 0 ) 83 84 img = Scalar::all(0); 85 //状态坐标白色 86 drawCross(statePt, Scalar(255, 255, 255), 3); 87 //测量坐标蓝色 88 drawCross(measPt, Scalar(0, 0, 255), 3); 89 //预测坐标绿色 90 drawCross(predictPt, Scalar(0, 255, 0), 3); 91 //真实值和测量值之间用红色线连接起来 92 line(img, statePt, measPt, Scalar(0, 0, 255), 3, CV_AA, 0); 93 //真实值和估计值之间用黄色线连接起来 94 line(img, statePt, predictPt, Scalar(0, 255, 255), 3, CV_AA, 0); 95 96 97 /* 98 使用kalma步骤三 99 调用kalman这个类的correct方法得到加入观察值校正后的状态变量值矩阵 100 */ 101 102 if (theRNG().uniform(0, 4) != 0) 103 KF.correct(measurement); 104 105 randn(processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0)))); 106 //不加噪声的话就是匀速圆周运动，加了点噪声类似匀速圆周运动，因为噪声的原因，运动方向可能会改变 107 state = KF.transitionMatrix*state + processNoise; 108 109 imshow("Kalman", img); 110 code = (char)waitKey(100); 111 112 if (code > 0) 113 break; 114 } 115 if (code == 27 || code == 'q' || code == 'Q') 116 break; 117 } 118 119 return 0; 120}