使用OpenCV和C++的卡尔曼滤波器

Kalman Filter using OpenCV and C++

本文关键字：滤波器 OpenCV C++ 使用      更新时间：2023-10-16

初始化：

    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是常量。

    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();

代码在predict时崩溃，该错误源自predict((函数中的statePre = transitionMatrix*statePost;。该错误是由于类型的断言失败所致。我使用transpose，因为t.estimated_state是statePost应该设置为的转置。我曾尝试使用convertTo((来更改t.estimated_state的类型。对statePost使用setTo((也不起作用。

有人能指出我哪里错了吗？

我能够解决我的问题。我应该将卡尔曼滤波器的类型设置为CV_64FC1，因为我提供的所有值都是double类型。我还将初始化transitionMatrix的Mat更改为类型double。这是我的最后一个代码

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);
相关文章：