1.kalman 对一个圆进行kalman追踪
kalman.pro
QT += core
QT -= gui
CONFIG += c++11
TARGET = kalman
CONFIG += console
CONFIG -= app_bundle
TEMPLATE = app
SOURCES += main.cpp
# The following define makes your compiler emit warnings if you use
# any feature of Qt which as been marked deprecated (the exact warnings
# depend on your compiler). Please consult the documentation of the
# deprecated API in order to know how to port your code away from it.
DEFINES += QT_DEPRECATED_WARNINGS
# You can also make your code fail to compile if you use deprecated APIs.
# In order to do so, uncomment the following line.
# You can also select to disable deprecated APIs only up to a certain version of Qt.
#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0
INCLUDEPATH += E:\opencv_3.2\build\install\include
LIBS += -LE:\opencv_3.2\build\install\x86\mingw\bin -lopencv_core320 -lopencv_highgui320 -lopencv_imgcodecs320 -lopencv_video320\
-lopencv_videoio320 -lopencv_imgproc320
main.cpp
#include "opencv2/video/tracking.hpp"
#include "opencv2/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 main(int, char**){ Mat img(500, 500, CV_8UC3); KalmanFilter KF(2, 1, 0); //创建卡尔曼滤波器对象KF Mat state(2, 1, CV_32F); /* (phi, delta_phi) */ //state(角度，△角度) 角度、角速度 Mat processNoise(2, 1, CV_32F); Mat measurement = Mat::zeros(1, 1, CV_32F); //定义测量值 char code = (char)-1; for(;;) { //1.初始化 randn( state, Scalar::all(0), Scalar::all(0.1) ); //随机生成一个矩阵，期望是0，标准差为0.1; KF.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1); // 转移矩阵 //setIdentity: 缩放的单位对角矩阵; // !< measurement matrix (H) 观测模型 //setIdentity函数是给参数矩阵对角线赋相同值，默认是1 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)); //预测估计协方差矩阵; //statePost为校正状态，其本质就是前一时刻的状态 randn(KF.statePost, Scalar::all(0), Scalar::all(0.1)); //x(0)初始化，产生均值为0，标准差为0.1的二维高斯列向量 for(;;) { Point2f center(img.cols*0.5f, img.rows*0.5f); //center图像中心点 float R = img.cols/3.f; //半径 double stateAngle = state.at<float>(0); //跟踪点角度，state中存放起始角，state为初始状态 Point statePt = calcPoint(center, R, stateAngle); //跟踪点坐标statePt //2. 预测 Mat prediction = KF.predict(); //计算预测值，返回x' 得到状态的预测值矩阵 double predictAngle = prediction.at<float>(0); //预测点的角度 Point predictPt = calcPoint(center, R, predictAngle); //预测点坐标predictPt //3.更新 measurement是测量值 randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0))); //给measurement赋值N(0,R)的随机值 // generate measurement measurement += KF.measurementMatrix*state; //z = z + H*x; 带噪声的测量 double measAngle = measurement.at<float>(0); Point measPt = calcPoint(center, R, measAngle); // plot points //定义了画十字的方法，值得学习下 #define drawCross( center, color, d ) line( img, Point( center.x - d, center.y - d ), Point( center.x + d, center.y + d ), color, 1, LINE_AA, 0); \ line( img, Point( center.x + d, center.y - d ), Point( center.x - d, center.y + d ), color, 1, LINE_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, LINE_AA, 0 ); //真实值与测量值之间用红色连接 line( img, statePt, predictPt, Scalar(0,255,255), 3, LINE_AA, 0 ); //真实值与估计值之间用黄色连接 //调用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;}
这样就能生成一个在圆上不断循环预测--更新的过程，实现了kalman的功能。
2. kalman对两个圆进行追踪
其中main.cpp
//对两个圆同时进行kalman追踪
#include "opencv2/video/tracking.hpp"
#include "opencv2/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 main(int, char**)
{
Mat img(500, 500, CV_8UC3);
KalmanFilter KF(2, 1, 0); //创建卡尔曼滤波器对象KF
Mat state(2, 1, CV_32F); //(phi, delta_phi) state(角度，△角度) 角度、角速度
Mat processNoise(2, 1, CV_32F);
Mat measurement = Mat::zeros(1, 1, CV_32F); //定义测量值
KalmanFilter KF2(2, 1, 0);
Mat state2(2, 1, CV_32F); // (phi, delta_phi) state(角度，△角度) 角度、角速度
Mat processNoise2(2, 1, CV_32F);
Mat measurement2 = Mat::zeros(1, 1, CV_32F); //定义测量值
char code = (char)-1;
for(;;)
{
//1.初始化
randn( state, Scalar::all(0), Scalar::all(0.1) ); //随机生成一个矩阵，期望是0，标准差为0.1;
KF.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1); // 转移矩阵
randn( state2, Scalar::all(0), Scalar::all(0.1) );
KF2.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1);
//setIdentity: 缩放的单位对角矩阵;
// !< measurement matrix (H) 观测模型
//setIdentity函数是给参数矩阵对角线赋相同值，默认是1
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)); //预测估计协方差矩阵;
setIdentity(KF2.measurementMatrix); //测量矩阵 H
setIdentity(KF2.processNoiseCov, Scalar::all(1e-5)); //系统噪声方差矩阵 Q
setIdentity(KF2.measurementNoiseCov, Scalar::all(1e-1)); //测量噪声方差矩阵 R
setIdentity(KF2.errorCovPost, Scalar::all(1)); //预测估计协方差矩阵;
//statePost为校正状态，其本质就是前一时刻的状态
randn(KF.statePost, Scalar::all(0), Scalar::all(0.1)); //x(0)初始化，产生均值为0，标准差为0.1的二维高斯列向量
randn(KF2.statePost, Scalar::all(0), Scalar::all(0.1));
for(;;)
{
Point2f center(img.cols*0.5f, img.rows*0.5f); //center图像中心点
float R = img.cols/3.f; //半径
double stateAngle = state.at<float>(0); //跟踪点角度，state中存放起始角，state为初始状态
Point statePt = calcPoint(center, R, stateAngle); //跟踪点坐标statePt
Point2f center2(img.cols*0.2f, img.rows*0.2f); //center图像中心点
float R2 = img.cols/3.f; //半径
double stateAngle2 = state2.at<float>(0); //跟踪点角度，state中存放起始角，state为初始状态
Point statePt2 = calcPoint(center2, R2, stateAngle2); //跟踪点坐标statePt
//2. 预测
Mat prediction = KF.predict(); //计算预测值，返回x' 得到状态的预测值矩阵
double predictAngle = prediction.at<float>(0); //预测点的角度
Point predictPt = calcPoint(center, R, predictAngle); //预测点坐标predictPt
Mat prediction2 = KF2.predict(); //计算预测值，返回x' 得到状态的预测值矩阵
double predictAngle2 = prediction2.at<float>(0); //预测点的角度
Point predictPt2 = calcPoint(center2, R2, predictAngle2); //预测点坐标predictPt
//3.更新 measurement是测量值
randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0))); //给measurement赋值N(0,R)的随机值
randn( measurement2, Scalar::all(0), Scalar::all(KF2.measurementNoiseCov.at<float>(0)));
// generate measurement
measurement += KF.measurementMatrix*state; //z = z + H*x; 带噪声的测量
measurement2 += KF2.measurementMatrix*state2;
double measAngle = measurement.at<float>(0);
Point measPt = calcPoint(center, R, measAngle);
double measAngle2 = measurement2.at<float>(0);
Point measPt2 = calcPoint(center2, R2, measAngle2);
// plot points //定义了画十字的方法，值得学习下
#define drawCross( center, color, d ) line( img, Point( center.x - d, center.y - d ), Point( center.x + d, center.y + d ), color, 1, LINE_AA, 0); \
line( img, Point( center.x + d, center.y - d ), Point( center.x - d, center.y + d ), color, 1, LINE_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, LINE_AA, 0 ); //真实值与测量值之间用红色连接
line( img, statePt, predictPt, Scalar(0,255,255), 3, LINE_AA, 0 ); //真实值与估计值之间用黄色连接
drawCross( statePt2, Scalar(255,255,255), 3 ); //状态坐标白色
drawCross( measPt2, Scalar(0,0,255), 3 ); //测量坐标蓝色
drawCross( predictPt2, Scalar(0,255,0), 3 ); //预测坐标绿色
line( img, statePt2, measPt2, Scalar(0,0,255), 3, LINE_AA, 0 ); //真实值与测量值之间用红色连接
line( img, statePt2, predictPt2, Scalar(0,255,255), 3, LINE_AA, 0 ); //真实值与估计值之间用黄色连接
//调用kalman这个类的correct方法得到加入观察值校正后的状态变量值矩阵
if(theRNG().uniform(0,4) != 0)
KF.correct(measurement); //校正
KF2.correct(measurement2);
//不加噪声的话就是匀速圆周运动，加了点噪声类似匀速圆周运动，因为噪声的原因，运动方向可能会改变
///*
randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));
state = KF.transitionMatrix*state + processNoise;
//*/
randn( processNoise2, Scalar(0), Scalar::all(sqrt(KF2.processNoiseCov.at<float>(0, 0))));
state2 = KF2.transitionMatrix*state2 + processNoise2;
imshow( "Kalman", img );
code = (char)waitKey(100);
if( code > 0 )
break;
}
if( code == 27 || code == 'q' || code == 'Q' )
break;
}
return 0;
}
结果：
