+
+ // TODO
+ // init KalmanFilter
+ this->kf.transitionMatrix = (Mat_<float>(4, 4) <<
+ 1, 0, 1, 0,
+ 0, 1, 0, 1,
+ 0, 0, 1, 0,
+ 0, 0, 0, 1);
+
+ this->kf.measurementMatrix = (Mat_<float>(2, 2) <<
+ 1, 0, 0, 0,
+ 0, 1, 0, 0);
+
+ this->kf.processNoiseCov = 1e-5 * Mat_<float>::eye(4, 4);
+ this->kf.measurementNoiseCov = 1e-1 * Mat_<float>::ones(2, 2);
+ this->kf.errorCovPost = 1. * Mat_<float>::ones(4, 4);