Ask Your Question
0

Kalman Filter - Assertion failed - Matrix types

asked 2016-11-17 02:22:58 -0600

CocoJambo gravatar image

updated 2016-11-17 02:54:17 -0600

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 ...
(more)
edit retag flag offensive close merge delete

Comments

same error with KF.predict() (w/o control Mat) ?

berak gravatar imageberak ( 2016-11-17 02:52:52 -0600 )edit

No error then, interesting...

CocoJambo gravatar imageCocoJambo ( 2016-11-17 02:57:09 -0600 )edit

^^ hmmm , now we've got something to debug ..

berak gravatar imageberak ( 2016-11-17 03:01:46 -0600 )edit

1 answer

Sort by ยป oldest newest most voted
1

answered 2016-11-17 03:16:03 -0600

berak gravatar image

updated 2016-11-17 03:17:05 -0600

you initialize your KF with CP=0, so the internal controlMatrix is emtpy (type==0)

if you now supply a control Mat in predict() , the matrix product here will throw a type exception

solution: either give it a valid CP value in init() (should be probably 3 in your case) or do not supply a control Mat in predict()

edit flag offensive delete link more

Comments

btw, maybe the controlMatrix should not be released at all . imho, having it around does not do any harm (even if it is never used)

berak gravatar imageberak ( 2016-11-17 03:20:46 -0600 )edit
1

just noticed it at the same time, my problem was twofold. First, i did not set the controlMatrix to identity. Second, setting it to identity will not do anything unless I give the init function the length of the control vector as you mentioned. So a problem of me using an internet example which didnt use control vectors in the first place! Thank you, berak.

CocoJambo gravatar imageCocoJambo ( 2016-11-17 03:21:33 -0600 )edit

hmm, ignore above comment, you'd need a valid CP value for initialization anyway

berak gravatar imageberak ( 2016-11-17 03:23:27 -0600 )edit
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2016-11-17 02:18:37 -0600

Seen: 106 times

Last updated: Nov 17 '16