iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
KalmanFilter.H
Go to the documentation of this file.
1 /*! @file
2  @author Dirk Walther, Zhihao Li
3  @copyright GNU Public License (GPL v3)
4  @section License
5  @verbatim
6  // ////////////////////////////////////////////////////////////////////////
7  // The iLab Neuromorphic Robotics Toolkit (NRT) //
8  // Copyright 2010-2012 by the University of Southern California (USC) //
9  // and the iLab at USC. //
10  // //
11  // iLab - University of Southern California //
12  // Hedco Neurociences Building, Room HNB-10 //
13  // Los Angeles, Ca 90089-2520 - USA //
14  // //
15  // See http://ilab.usc.edu for information about this project. //
16  // ////////////////////////////////////////////////////////////////////////
17  // This file is part of The iLab Neuromorphic Robotics Toolkit. //
18  // //
19  // The iLab Neuromorphic Robotics Toolkit is free software: you can //
20  // redistribute it and/or modify it under the terms of the GNU General //
21  // Public License as published by the Free Software Foundation, either //
22  // version 3 of the License, or (at your option) any later version. //
23  // //
24  // The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
25  // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
26  // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
27  // PURPOSE. See the GNU General Public License for more details. //
28  // //
29  // You should have received a copy of the GNU General Public License //
30  // along with The iLab Neuromorphic Robotics Toolkit. If not, see //
31  // <http://www.gnu.org/licenses/>. //
32  // ////////////////////////////////////////////////////////////////////////
33  @endverbatim */
34 
35 
36 #ifndef INCLUDE_NRT_IMAGEPROC_FILTERING_KALMANFILTER_H
37 #define INCLUDE_NRT_IMAGEPROC_FILTERING_KALMANFILTER_H
38 
39 #include <Eigen/Core>
40 
41 namespace nrt
42 {
43 #define DEF_PNOISE 1.0F
44 #define DEF_MNOISE 1.0F
45 #define DEF_TSTEP 1.0F
46 
47  //! implementation of a 2nd order linear Kalman Filter
48  /*! This class can be used for tracking. The state vector is second order,
49  i.e. it maintains location, speed, and accelaration to generate
50  predictions.*/
52  {
53  public:
54 
55  //! default constructor - need to call init before the filter can be used
56  KalmanFilter();
57 
58  //! constructor that initializes the filter
59  /*!@param initialM - the initial measurement used to jump start the filter
60  @param pNoise - std of the (Gaussian) proess noise
61  @param mNoise - std of the (Gaussian) measurement noise
62  @param timeStep - the time interval between measurements (default: 1)*/
63  KalmanFilter(float initialM, float pNoise = DEF_PNOISE,
64  float mNoise = DEF_MNOISE, float timeStep = DEF_TSTEP);
65 
66  // default copy constructor and destructor are fine
67 
68  //! initialize the filter
69  /*!@param initialM - the initial measurement used to jump start the filter
70  @param pNoise - std of the (Gaussian) process noise
71  @param mNoise - std of the (Gaussian) measurement noise
72  @param timeStep - the time interval between measurements (default: 1)*/
73  void init(float initialM, float pNoise = DEF_PNOISE,
74  float mNoise = DEF_MNOISE, float timeStep = DEF_TSTEP);
75 
76  // //! write the entire KalmanFilter to the output stream os
77  // void writeToStream(std::ostream& os) const;
78 
79  // //! read the KalmanFilter from the input stream is
80  // void readFromStream(std::istream& is);
81 
82  // //! write the entire KalmanFilter to the cereal archive ar
83  // template <class Archive>
84  // void writeToArchive(Archive & ar) const;
85 
86  // //! read the KalmanFilter from the cereal archive ar
87  // template <class Archive>
88  // void readFromArchive(Archive & ar);
89 
90  //! returns a prediction for the next value
91  float getEstimate() const;
92 
93  //! returns a prediction for the next value given a measurement
94  float getEstimate(float measurement) const;
95 
96  //! returns the speed (second entry in the state vector)
97  float getSpeed() const;
98 
99  //! returns the cost for associating measurement with this Kalman tracker
100  float getCost(float measurement) const;
101 
102  //! update the filter for the next time step without a measurement (skipped value)
103  float update();
104 
105  //! update the filter for the next time step with a measurement
106  float update(float measurement);
107 
108  //! returns the current state vector [x, v, a]
109  Eigen::Vector3f getStateVector() const;
110 
111  //! returns the covariance matrix P
112  Eigen::Matrix3f getCovariances() const;
113 
114  //! test whether the filter is initialized
115  bool isInitialized() const;
116 
117 
118  private:
119  // update the Kalman matrix and the covariance matrices
120  void updateFilter();
121 
122  // returns the predicted state vector without a measurement
123  Eigen::Vector3f getXEstimate() const;
124 
125  // returns the predicted state vector given a measurement
126  Eigen::Vector3f getXEstimate(float z) const;
127 
128 
129  // a number of matrices used for the processing
130  Eigen::RowVector3f H;
131  Eigen::Vector3f x, HT, K;
132  Eigen::Matrix3f I, P, Phi, PhiT, Q, M;
133  float itsPNoise, itsMNoise2;
134  bool initialized;
135  };
136 }; // namespace nrt
137 
138 // #include <nrt/ImageProc/Filtering/details/KalmanFilterImpl.H>
139 
140 #endif // INCLUDE_NRT_IMAGEPROC_FILTERING_KALMANFILTER_H