Kalman.cpp
Go to the documentation of this file.
00001 // Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
00002 // Refer to README.md in this directory.
00003 
00004 #include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h>
00005 #include "opencv2/opencv.hpp"
00006 #include <iostream>
00007 #include <vector>
00008 
00009 //---------------------------------------------------------------------------
00010 //---------------------------------------------------------------------------
00011 TKalmanFilter::TKalmanFilter(Point_t pt, track_t deltatime)
00012 {
00013   // time increment (lower values makes target more "massive")
00014   dt = deltatime;
00015 
00016   // 6 state variables [x y z xdot ydot zdot], 3 measurements [x y z]
00017   kalman = new cv::KalmanFilter(6, 3, 0);
00018   // Transition cv::Matrix
00019   kalman->transitionMatrix = (cv::Mat_<track_t>(6, 6) <<
00020                               1, 0, 0, dt,  0,  0,
00021                               0, 1, 0,  0, dt,  0,
00022                               0, 0, 1,  0,  0, dt,
00023                               0, 0, 0,  1,  0,  0,
00024                               0, 0, 0,  0,  1,  0,
00025                               0, 0, 0,  0,  0,  1);
00026   // init...
00027   LastPosition = pt;
00028   kalman->statePre.at<track_t>(0) = pt.x;
00029   kalman->statePre.at<track_t>(1) = pt.y;
00030   kalman->statePre.at<track_t>(2) = pt.z;
00031 
00032   kalman->statePre.at<track_t>(3) = 0;
00033   kalman->statePre.at<track_t>(4) = 0;
00034   kalman->statePre.at<track_t>(5) = 0;
00035 
00036   kalman->statePost.at<track_t>(0) = pt.x;
00037   kalman->statePost.at<track_t>(1) = pt.y;
00038   kalman->statePost.at<track_t>(2) = pt.z;
00039 
00040   // Nur x, y und z Koordinaten messbar!
00041   kalman->measurementMatrix = (cv::Mat_<track_t>(3, 6) <<
00042                                1, 0, 0, 0, 0, 0,
00043                                0, 1, 0, 0, 0, 0,
00044                                0, 0, 1, 0, 0, 0);
00045 
00046   float sigma = 0.5; // Assume standard deviation for acceleration, todo: dynamic reconfigure
00047   float c1 = pow(dt,4.0)/4.0;
00048   float c2 = pow(dt,2.0);
00049   float c3 = pow(dt,3.0)/2.0;
00050   kalman->processNoiseCov = sigma*sigma*(cv::Mat_<float>(6, 6) <<
00051                              c1,  0,  0, c3,  0,  0,
00052                               0, c1,  0,  0, c3,  0,
00053                               0,  0, c1,  0,  0, c3,
00054                              c3,  0,  0, c2,  0,  0,
00055                               0, c3,  0,  0, c2,  0,
00056                               0,  0, c3,  0,  0, c2);
00057 
00058   cv::setIdentity(kalman->measurementNoiseCov, cv::Scalar::all(5e-2));
00059 
00060   cv::setIdentity(kalman->errorCovPost, cv::Scalar::all(1000000));
00061 }
00062 //---------------------------------------------------------------------------
00063 TKalmanFilter::~TKalmanFilter() { delete kalman; }
00064 
00065 //---------------------------------------------------------------------------
00066 void TKalmanFilter::Prediction()
00067 {
00068   cv::Mat prediction = kalman->predict();
00069   LastPosition = Point_t(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2));
00070   LastVelocity = Point_t(prediction.at<track_t>(3), prediction.at<track_t>(4), prediction.at<track_t>(5));
00071 }
00072 
00073 //---------------------------------------------------------------------------
00074 Point_t TKalmanFilter::Update(Point_t p, bool DataCorrect)
00075 {
00076   cv::Mat measurement(3, 1, Mat_t(1));
00077   if (!DataCorrect)
00078   {
00079     measurement.at<track_t>(0) = LastPosition.x; // update using prediction
00080     measurement.at<track_t>(1) = LastPosition.y;
00081     measurement.at<track_t>(2) = LastPosition.z;
00082   }
00083   else
00084   {
00085     measurement.at<track_t>(0) = p.x; // update using measurements
00086     measurement.at<track_t>(1) = p.y;
00087     measurement.at<track_t>(2) = p.z;
00088   }
00089   // Correction
00090   cv::Mat estimated = kalman->correct(measurement);
00091   LastPosition.x = estimated.at<track_t>(0); // update using measurements
00092   LastPosition.y = estimated.at<track_t>(1);
00093   LastPosition.z = estimated.at<track_t>(2);
00094   LastVelocity.x = estimated.at<track_t>(3);
00095   LastVelocity.y = estimated.at<track_t>(4);
00096   LastVelocity.z = estimated.at<track_t>(5);
00097   return LastPosition;
00098 }
00099 //---------------------------------------------------------------------------


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Sat Jun 8 2019 20:53:15