10 kalman =
new KalmanFilter( 4, 4, 0 );
11 kalman->transitionMatrix = (Mat_<float>(4, 4) << 1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1);
15 kalman->statePre.at<
float>(0) = p[0][0];
16 kalman->statePre.at<
float>(1) = p[0][1];
17 kalman->statePre.at<
float>(2) = p[1][0];
18 kalman->statePre.at<
float>(3) = p[1][1];
20 kalman->statePost.at<
float>(0)=p[0][0];
21 kalman->statePost.at<
float>(1)=p[0][1];
22 kalman->statePost.at<
float>(2)=p[1][0];
23 kalman->statePost.at<
float>(3)=p[1][1];
26 setIdentity(kalman->processNoiseCov, Scalar::all(1e-4));
27 setIdentity(kalman->measurementNoiseCov, Scalar::all(1e-1));
28 setIdentity(kalman->errorCovPost, Scalar::all(5));
38 Mat prediction = kalman->predict();
39 prevResult[0][0] = prediction.at<
float>(0);prevResult[0][1] = prediction.at<
float>(1);
40 prevResult[1][0] = prediction.at<
float>(2);prevResult[1][1] = prediction.at<
float>(3);
50 measurement.setTo(Scalar(0));
52 measurement.at<
float>(0) = measure[0][0];measurement.at<
float>(1) = measure[0][1];
53 measurement.at<
float>(2) = measure[1][0];measurement.at<
float>(3) = measure[1][1];
55 Mat estimated = kalman->correct(measurement);
58 if(estimated.at<
float>(0) < estimated.at<
float>(2)){
59 measure[0][0] = estimated.at<
float>(0);measure[0][1] = estimated.at<
float>(1);
60 measure[1][0] = estimated.at<
float>(2);measure[1][1] = estimated.at<
float>(3);
63 measure[0][0] = estimated.at<
float>(2);measure[0][1] = estimated.at<
float>(3);
64 measure[1][0] = estimated.at<
float>(0);measure[1][1] = estimated.at<
float>(1);
void setIdentity(geometry_msgs::Transform &tx)
cv::Mat_< float > measurement(2, 1)
vector< Vec2f > update(vector< Vec2f >)
CKalmanFilter(vector< Vec2f >)
vector< Vec2f > predict()