Go to the documentation of this file.00001 #include <iostream>
00002 #include "CKalmanFilter.h"
00003
00004 using namespace cv;
00005 using namespace std;
00006
00007
00008 CKalmanFilter::CKalmanFilter(vector<Vec2f> p){
00009
00010 kalman = new KalmanFilter( 4, 4, 0 );
00011 kalman->transitionMatrix = (Mat_<float>(4, 4) << 1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1);
00012
00013
00014 prevResult = p;
00015 kalman->statePre.at<float>(0) = p[0][0];
00016 kalman->statePre.at<float>(1) = p[0][1];
00017 kalman->statePre.at<float>(2) = p[1][0];
00018 kalman->statePre.at<float>(3) = p[1][1];
00019
00020 kalman->statePost.at<float>(0)=p[0][0];
00021 kalman->statePost.at<float>(1)=p[0][1];
00022 kalman->statePost.at<float>(2)=p[1][0];
00023 kalman->statePost.at<float>(3)=p[1][1];
00024
00025 setIdentity(kalman->measurementMatrix);
00026 setIdentity(kalman->processNoiseCov, Scalar::all(1e-4));
00027 setIdentity(kalman->measurementNoiseCov, Scalar::all(1e-1));
00028 setIdentity(kalman->errorCovPost, Scalar::all(5));
00029 }
00030
00031
00032 CKalmanFilter::~CKalmanFilter(){
00033 delete kalman;
00034 }
00035
00036
00037 vector<Vec2f> CKalmanFilter::predict(){
00038 Mat prediction = kalman->predict();
00039 prevResult[0][0] = prediction.at<float>(0);prevResult[0][1] = prediction.at<float>(1);
00040 prevResult[1][0] = prediction.at<float>(2);prevResult[1][1] = prediction.at<float>(3);
00041 return prevResult;
00042
00043 }
00044
00045
00046 vector<Vec2f> CKalmanFilter::update(vector<Vec2f> measure){
00047
00048
00049 Mat_<float> measurement(4,1);
00050 measurement.setTo(Scalar(0));
00051
00052 measurement.at<float>(0) = measure[0][0];measurement.at<float>(1) = measure[0][1];
00053 measurement.at<float>(2) = measure[1][0];measurement.at<float>(3) = measure[1][1];
00054
00055 Mat estimated = kalman->correct(measurement);
00056
00057
00058 if(estimated.at<float>(0) < estimated.at<float>(2)){
00059 measure[0][0] = estimated.at<float>(0);measure[0][1] = estimated.at<float>(1);
00060 measure[1][0] = estimated.at<float>(2);measure[1][1] = estimated.at<float>(3);
00061 }
00062 else{
00063 measure[0][0] = estimated.at<float>(2);measure[0][1] = estimated.at<float>(3);
00064 measure[1][0] = estimated.at<float>(0);measure[1][1] = estimated.at<float>(1);
00065 }
00066
00067 waitKey(1);
00068
00069 return measure;
00070
00071 }