Kalman Filter - Assertion failed - Matrix types
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 ...
same error with
KF.predict()
(w/o control Mat) ?No error then, interesting...
^^ hmmm , now we've got something to debug ..