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;
}
