Kalman Filter - Assertion failed - Matrix types

edit

Hey, I try to build a kalman filter and get runtime errors at the step Mat prediction = KF.predict(control); in regards to the matrix types. I checked all the types of the input matrices and they are of type 5 == CV_32FC1, which should be the correct one. But I assume I make some CV related mistake that I am unaware of. Any help is appreciated, even if you dont want to dig into the code.

Relevant code:

#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <tf/transform_datatypes.h>
#include "localizer_mm/localizer_mm_lib.h"

using namespace cv;
using namespace localizer_mm;

cv::KalmanFilter KF;
cv::Mat measurement(3,1,CV_32FC1);
cv::Mat state(3,1,CV_32FC1); // (x, y, psi)
cv::Mat pred(3,1,CV_32FC1);
cv::Mat corr(3,1,CV_32FC1);
cv::Mat control(3,1,CV_32FC1);
bool init = true;
Pose adma_pose, adma_pose_last, odom_pose, mama_pose,result;

void initKalman(float x, float y, float psi)
{
    // Initialize Kalman Filter
    KF.init(3, 3, 0);
    // initialize measurement, prior and posterior state
    measurement = Mat::zeros(3,1,CV_32FC1);
    measurement.at<float>(0,0) = x;
    measurement.at<float>(1,0) = y;
    measurement.at<float>(2,0) = psi;

    KF.statePre.setTo(0);
    KF.statePre.at<float>(0,0) = x;
    KF.statePre.at<float>(1,0) = y;
    KF.statePre.at<float>(2,0) = psi;

    KF.statePost.setTo(0);
    KF.statePost.at<float>(0,0) = x;
    KF.statePost.at<float>(1,0) = y;
    KF.statePost.at<float>(2,0) = psi;

    // initialize A, H, Q, R, P
    setIdentity(KF.transitionMatrix); // A
    setIdentity(KF.measurementMatrix); // H
    setIdentity(KF.processNoiseCov, Scalar::all(.005)); // Q
    setIdentity(KF.measurementNoiseCov, Scalar::all(.1)); // R
    setIdentity(KF.errorCovPost, Scalar::all(.1)); // P posterior
}

Mat kalmanPredict(float x, float y, float psi)
{
    std::cout << "measurement = \n";
    std::cout << measurement << "\n";
    std::cout << "statePre = \n";
    std::cout << KF.statePre << "\n";
    std::cout << "statePost = \n";
    std::cout << KF.statePost << "\n";
    std::cout << "transitionMatrix = \n";
    std::cout << KF.transitionMatrix << "\n";
    std::cout << "measurementMatrix = \n";
    std::cout << KF.measurementMatrix << "\n";
    std::cout << "processNoiseCov = \n";
    std::cout << KF.processNoiseCov << "\n";
    std::cout << "measurementNoiseCov = \n";
    std::cout << KF.measurementNoiseCov << "\n";
    std::cout << "errorCovPost = \n";
    std::cout << KF.errorCovPost << "\n";

    control = Mat::zeros(3,1,CV_32FC1);
    control.at<float>(0,0) = x;
    control.at<float>(1,0) = y;
    control.at<float>(2,0) = psi;
    std::cout << "control = \n";
    std::cout << control << "\n";
    Mat prediction = KF.predict(control); //ERROR
    std::cout << "finished prediction step\n";
    return prediction;
}

Mat kalmanCorrect(float x, float y, float psi)
{
    measurement.at<float>(0,0) = x;
    measurement.at<float>(1,0) = y;
    measurement.at<float>(2,0) = psi;
    Mat correction = KF.correct(measurement);
    return correction;
}

void localizerCallback(const localization_msgs::Pose2DWithCovarianceStampedConstPtr &matcher_pose)
{
    ROS_INFO("entering localizerCallback callback");
    // initialize Kalman Filter
    if (init == true) initKalman((float)adma_pose.x,(float)adma_pose.y,(float)adma_pose.psi);
    init = false;

    // convert messages, [x y psi] are double values
    odom_pose.x = adma_pose.x - adma_pose_last.x;
    odom_pose.y = adma_pose.y - adma_pose_last.y;
    odom_pose.psi = adma_pose.psi - adma_pose_last.psi;
    msg2Pose(matcher_pose, mama_pose);

    // apply prediction and correction step
    pred = kalmanPredict((float ...