Kalman.cpp
Go to the documentation of this file.
1 // Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
2 // Refer to README.md in this directory.
3 
5 #include "opencv2/opencv.hpp"
6 #include <iostream>
7 #include <vector>
8 
9 //---------------------------------------------------------------------------
10 //---------------------------------------------------------------------------
12 {
13  // time increment (lower values makes target more "massive")
14  dt = deltatime;
15 
16  // 6 state variables [x y z xdot ydot zdot], 3 measurements [x y z]
17  kalman = new cv::KalmanFilter(6, 3, 0);
18  // Transition cv::Matrix
19  kalman->transitionMatrix = (cv::Mat_<track_t>(6, 6) <<
20  1, 0, 0, dt, 0, 0,
21  0, 1, 0, 0, dt, 0,
22  0, 0, 1, 0, 0, dt,
23  0, 0, 0, 1, 0, 0,
24  0, 0, 0, 0, 1, 0,
25  0, 0, 0, 0, 0, 1);
26  // init...
27  LastPosition = pt;
28  kalman->statePre.at<track_t>(0) = pt.x;
29  kalman->statePre.at<track_t>(1) = pt.y;
30  kalman->statePre.at<track_t>(2) = pt.z;
31 
32  kalman->statePre.at<track_t>(3) = 0;
33  kalman->statePre.at<track_t>(4) = 0;
34  kalman->statePre.at<track_t>(5) = 0;
35 
36  kalman->statePost.at<track_t>(0) = pt.x;
37  kalman->statePost.at<track_t>(1) = pt.y;
38  kalman->statePost.at<track_t>(2) = pt.z;
39 
40  // Nur x, y und z Koordinaten messbar!
41  kalman->measurementMatrix = (cv::Mat_<track_t>(3, 6) <<
42  1, 0, 0, 0, 0, 0,
43  0, 1, 0, 0, 0, 0,
44  0, 0, 1, 0, 0, 0);
45 
46  float sigma = 0.5; // Assume standard deviation for acceleration, todo: dynamic reconfigure
47  float c1 = pow(dt,4.0)/4.0;
48  float c2 = pow(dt,2.0);
49  float c3 = pow(dt,3.0)/2.0;
50  kalman->processNoiseCov = sigma*sigma*(cv::Mat_<float>(6, 6) <<
51  c1, 0, 0, c3, 0, 0,
52  0, c1, 0, 0, c3, 0,
53  0, 0, c1, 0, 0, c3,
54  c3, 0, 0, c2, 0, 0,
55  0, c3, 0, 0, c2, 0,
56  0, 0, c3, 0, 0, c2);
57 
58  cv::setIdentity(kalman->measurementNoiseCov, cv::Scalar::all(5e-2));
59 
60  cv::setIdentity(kalman->errorCovPost, cv::Scalar::all(1000000));
61 }
62 //---------------------------------------------------------------------------
64 
65 //---------------------------------------------------------------------------
67 {
68  cv::Mat prediction = kalman->predict();
69  LastPosition = Point_t(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2));
70  LastVelocity = Point_t(prediction.at<track_t>(3), prediction.at<track_t>(4), prediction.at<track_t>(5));
71 }
72 
73 //---------------------------------------------------------------------------
75 {
76  cv::Mat measurement(3, 1, Mat_t(1));
77  if (!DataCorrect)
78  {
79  measurement.at<track_t>(0) = LastPosition.x; // update using prediction
80  measurement.at<track_t>(1) = LastPosition.y;
81  measurement.at<track_t>(2) = LastPosition.z;
82  }
83  else
84  {
85  measurement.at<track_t>(0) = p.x; // update using measurements
86  measurement.at<track_t>(1) = p.y;
87  measurement.at<track_t>(2) = p.z;
88  }
89  // Correction
90  cv::Mat estimated = kalman->correct(measurement);
91  LastPosition.x = estimated.at<track_t>(0); // update using measurements
92  LastPosition.y = estimated.at<track_t>(1);
93  LastPosition.z = estimated.at<track_t>(2);
94  LastVelocity.x = estimated.at<track_t>(3);
95  LastVelocity.y = estimated.at<track_t>(4);
96  LastVelocity.z = estimated.at<track_t>(5);
97  return LastPosition;
98 }
99 //---------------------------------------------------------------------------
~TKalmanFilter()
Definition: Kalman.cpp:63
Point_t Update(Point_t p, bool DataCorrect)
Definition: Kalman.cpp:74
#define Mat_t
Definition: defines.h:9
cv::Point3_< track_t > Point_t
Definition: defines.h:8
cv::KalmanFilter * kalman
Definition: Kalman.h:16
Point_t LastPosition
Definition: Kalman.h:18
track_t dt
Definition: Kalman.h:17
TKalmanFilter(Point_t p, track_t deltatime=0.2)
Definition: Kalman.cpp:11
float track_t
Definition: defines.h:7
Point_t LastVelocity
Definition: Kalman.h:19
void Prediction()
Definition: Kalman.cpp:66


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Sat May 16 2020 03:19:18