Kalman Filter using OpenCV and C++

Question

Initialization:

    p->kalman_filter = new cv::KalmanFilter(state_dim, measurement_dim, 0);
    p->kalman_filter->transitionMatrix = *(cv::Mat_<float>(state_dim, state_dim)
                                                  << 1,0,1,0, 0,1,0,1,
                                                  0,0,TIME_DIFFERENCE,0,
                                                  0,0,0,TIME_DIFFERENCE);
    setIdentity(p->kalman_filter->measurementMatrix);
    setIdentity(p->kalman_filter->processNoiseCov, cv::Scalar::all(1e-4));
    setIdentity(p->kalman_filter->measurementNoiseCov, cv::Scalar::all(1e-1));
    setIdentity(p->kalman_filter->errorCovPost, cv::Scalar::all(.1));

TIME_DIFFERENCE is constant.

    cv::Mat new_state;
    track t = p->tracks.at(track_id);

    cv::transpose((cv::Mat)t.estimated_state, p->kalman_filter->statePost);
    t.estimated_error_covariance.copyTo(p->kalman_filter->errorCovPost);

    new_state = p->kalman_filter->predict();

The code crashes at predict, the error originating from statePre = transitionMatrix*statePost; in the predict() function. The error is due to the failure of the assertion for type. I am using transpose as the t.estimated_state is the transpose of what the statePost should be set to.I have tried using convertTo() to change the type of the t.estimated_state.Using setTo() for statePost also doesn't work.

Could someone point out where I am going wrong?

user2522981 user2522981 · Accepted Answer · 2016-09-29T14:43:13

I was able to solve my problem. I was supposed to set the type of the Kalman Filter to CV_64FC1 as all the values I was providing were of type double. I also changed the Mat initializing the transitionMatrix to type double. This is my final code

p->kalman_filter = new cv::KalmanFilter(state_dim, measurement_dim, 0, CV_64FC1);
p->kalman_filter->transitionMatrix = *(cv::Mat_<double>(state_dim, state_dim)
                                                  << 1,0,1,0, 0,1,0,1,
                                                  0,0,TIME_DIFFERENCE,0,
                                                  0,0,0,TIME_DIFFERENCE);

Kalman Filter using OpenCV and C++

1 Answers