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

【问题标题】：Kalman Filter using OpenCV and C++使用 OpenCV 和 C++ 的卡尔曼滤波器
【发布时间】：2017-02-06 13:28:40
【问题描述】：

初始化：

    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;。该错误是由于类型断言失败造成的。我正在使用转置，因为 t.estimated_state 是 statePost 应该设置的转置。我尝试使用 convertTo() 来更改 t.estimated_state 的类型。对 statePost 使用 setTo() 也不起作用。

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

【问题讨论】：

标签： c++ opencv kalman-filter

【解决方案1】：

我能够解决我的问题。我应该将卡尔曼滤波器的类型设置为 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);

【讨论】：