JeVoisBase  1.20
JeVois Smart Embedded Machine Vision Toolkit Base Modules
Share this page:
Kalman1D.C
Go to the documentation of this file.
1 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2 //
3 // JeVois Smart Embedded Machine Vision Toolkit - Copyright (C) 2016 by Laurent Itti, the University of Southern
4 // California (USC), and iLab at USC. See http://iLab.usc.edu and http://jevois.org for information about this project.
5 //
6 // This file is part of the JeVois Smart Embedded Machine Vision Toolkit. This program is free software; you can
7 // redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software
8 // Foundation, version 2. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
9 // without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public
10 // License for more details. You should have received a copy of the GNU General Public License along with this program;
11 // if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
12 //
13 // Contact information: Laurent Itti - 3641 Watt Way, HNB-07A - Los Angeles, CA 90089-2520 - USA.
14 // Tel: +1 213 740 3527 - itti@pollux.usc.edu - http://iLab.usc.edu - http://jevois.org
15 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
16 /*! \file */
17 
20 #include <cmath>
21 
22 // ####################################################################################################
23 Kalman1D::Kalman1D(std::string const & instance) :
24  jevois::Component(instance), itsKF(2, 1, 0), itsState(2, 1, CV_32F), itsProcessNoise(2, 1, CV_32F),
25  itsMeasurement(1, 1, CV_32F), itsLatest(1, 1, CV_32F), itsFresh(false)
26 {
27  cv::setIdentity(itsKF.measurementMatrix);
28 }
29 
30 // ####################################################################################################
32 { }
33 
34 // ####################################################################################################
36 {
37  itsKF.statePost.at<float>(0) = 0.0F; // middle of screen
38  itsKF.statePost.at<float>(1) = 0.0F; // not moving
39 
40  // Predict once to get rolling:
41  itsKF.predict();
42 }
43 
44 // ####################################################################################################
45 void Kalman1D::onParamChange(kalman1d::usevel const & JEVOIS_UNUSED_PARAM(param), bool const & newval)
46 {
47  if (newval) itsKF.transitionMatrix = (cv::Mat_<float>(2, 2) << 1,1, 0,1);
48  else itsKF.transitionMatrix = (cv::Mat_<float>(2, 2) << 1,0, 0,1);
49 }
50 
51 // ####################################################################################################
52 void Kalman1D::onParamChange(kalman1d::procnoise const & JEVOIS_UNUSED_PARAM(param), float const & newval)
53 {
54  itsKF.processNoiseCov = (cv::Mat_<float>(2, 2) << newval * newval, 0, 0, newval * newval );
55 }
56 
57 // ####################################################################################################
58 void Kalman1D::onParamChange(kalman1d::measnoise const & JEVOIS_UNUSED_PARAM(param), float const & newval)
59 {
60  cv::setIdentity(itsKF.measurementNoiseCov, cv::Scalar::all(newval * newval));
61 }
62 
63 // ####################################################################################################
64 void Kalman1D::onParamChange(kalman1d::postnoise const & JEVOIS_UNUSED_PARAM(param), float const & newval)
65 {
66  cv::setIdentity(itsKF.errorCovPost, cv::Scalar::all(newval * newval));
67 }
68 
69 // ####################################################################################################
70 void Kalman1D::set(float x)
71 {
72  itsMeasurement.at<float>(0) = x;
73  itsLatest = itsKF.correct(itsMeasurement);
74  itsFresh = true; // itsLatest is "fresh", no need to predict() at the next get()
75 }
76 
77 // ####################################################################################################
78 float Kalman1D::get(float const eps)
79 {
80  // If we just had a fresh set(), use our latest result form correct(), otherwise predict the next position:
81  float x;
82  if (itsFresh)
83  {
84  x = itsLatest.at<float>(0);
85  itsFresh = false;
86 
87  // Now predict and update itsLatest:
88  itsLatest = itsKF.predict();
89  }
90  else
91  {
92  // First predict and update itsLatest:
93  itsLatest = itsKF.predict();
94 
95  x = itsLatest.at<float>(0);
96  }
97 
98  if (eps) x = std::round(x / eps) * eps;
99 
100  return x;
101 }
102 
Kalman1D::itsMeasurement
cv::Mat itsMeasurement
Definition: Kalman1D.H:86
Coordinates.H
Kalman1D::~Kalman1D
virtual ~Kalman1D()
Destructor.
Definition: Kalman1D.C:31
Kalman1D::Kalman1D
Kalman1D(std::string const &instance)
Constructor.
Definition: Kalman1D.C:23
Kalman1D::itsFresh
bool itsFresh
Definition: Kalman1D.H:88
jevois
F
float F
Kalman1D::itsKF
cv::KalmanFilter itsKF
Definition: Kalman1D.H:83
Kalman1D::itsLatest
cv::Mat itsLatest
Definition: Kalman1D.H:87
Kalman1D::onParamChange
void onParamChange(kalman1d::usevel const &param, bool const &newval) override
Kalman1D::set
void set(float x)
Function to call each time you have a new measurement.
Definition: Kalman1D.C:70
Kalman1D::get
float get(float const eps=0.01F)
Function to call each time you want to get the Kalman-filtered coordinates.
Definition: Kalman1D.C:78
Kalman1D::postInit
void postInit() override
Definition: Kalman1D.C:35
Kalman1D.H