iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
UKF.H
Go to the documentation of this file.
1 /*! @file
2  @author Randolph Voorhies (voorhies at usc dot edu)
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_PROBABILISTIC_BAYESFILTERS_UKF_H
37 #define INCLUDE_NRT_PROBABILISTIC_BAYESFILTERS_UKF_H
38 
39 #include <nrt/Eigen/Eigen.H>
40 
43 #include <Eigen/Cholesky>
44 
45 namespace nrt
46 {
47  //! An Unscented Kalman Filter
48  /*! The Unscented Kalman Filter is a very general and robust filter used for
49  prediction and tracking of potentially non-linear states. The
50  implementation here requires only that you provide definitions of the
51  state and the observation, as well as a filter model containing two
52  functions.
53 
54  \tparam StateDefinitionType An nrt::StateDefinition defining the fields
55  of the target to be tracked. For example, this State Definition may
56  include the current position, velocity and acceleration of a tracked
57  vehicle.
58 
59  \tparam ObservationDefinitionType An nrt::StateDefinition defining the
60  fields that your sensor will return. For example, if you are tracking
61  vehicles with a radar system, this State Definition may include the
62  measured range and bearing from the radar sensor to the target.
63 
64  \tparam FilterModel A simple struct which defines how your tracked target changes over time,
65  and how your sensor observes it. Read on for more information.
66 
67  \par The Filter Model
68 
69  The FilterModel template parameter should be a struct which you define to
70  tell the UKF how your target interacts with the world, and how the world
71  senses your target. The FilterModel that you provide must have the following definition:
72 
73 @code
74 // In this example, we model our target as a vehicle with x,y,theta position
75 // and translational,angular velocity
76 NRT_CREATE_STATE_FIELD_GROUP(Position, (x)(y)(theta));
77 NRT_CREATE_STATE_FIELD_GROUP(Velocity, (translational)(angular));
78 typedef nrt::StateDefinition<Position, Velocity> VehicleState;
79 
80 // ... And, we observe our vehicle with a range and angle
81 NRT_CREATE_STATE_FIELD_GROUP(PolarPosition, (range)(angle));
82 typedef nrt::StateDefinition<PolarPosition> VehicleObservation;
83 
84 struct MyCoolFilterModel
85 {
86 
87  // This can be any kind of custom data that your predict method may need to
88  // help it make predictions. For example, if you know the positions of the
89  // gas and brake pedals in the tracked vehicle (which would certainly help
90  // with the tracking!), then your ControlType struct could contain these
91  // values.
92  struct ControlType;
93 
94  // A function which takes as arguments a state of your target from the last timestep, and a control struct,
95  // and must return the predicted position for the current timestep. In our example, we should integrate the
96  // Velocity::translational and Velocity::angular into the Position::x, Position::y, and Position::theta
97  static nrt::StateVector<VehicleState>
98  predict(nrt::StateVector<VehicleState> const & state, ControlType const & control);
99 
100  // A function which takes as an argument a state of the target, and must return what you expect your sensor would retrun
101  // if your target was in this state. In our example, we should return the PolarPosition::range and PolarPosition::angle
102  // from the given state to the radar station.
103  static nrt::StateVector<VehicleObservation>
104  observe(nrt::StateVector<VehicleState> const & state)
105 };
106 @endcode
107 
108  \note The Unscented Kalman Filter works by running the predict and observe
109  methods for a few well-chosen points around the current believed state in
110  order to estimate how the state is moving. Because of this, you should
111  make sure that your predict and observe methods are completely
112  re-entrant.
113 
114  The neat thing about the UKF is that it can automatically predict
115  portions of your state which are not directly observable by your sensor.
116  In our vehicle tracking example above, notice that the sensor cannot
117  measure the speed or orientation of the vehicle - only the position.
118  However, the filter will quickly start estimating the orientation and
119  velocity fields automatically after a few timesteps.
120 
121  \ingroup probabalistic
122  \see probabalistic
123  */
124  template <class StateDefinitionType, class ObservationDefinitionType, class FilterModel>
125  class UKF
126  {
127  private:
128  const static size_t itsNS = StateDefinitionType::num_fields;
129  const static size_t itsNO = ObservationDefinitionType::num_fields;
130 
131  public:
132  //! Construct an Unscented Kalman Filter
133  UKF(GaussianPDF<StateDefinitionType> const & initialState,
134  Eigen::Matrix<double, itsNS, itsNS> const & predictionCovariance,
135  double alpha = 1e-3, double k = 0, double beta = 2.0);
136 
137  //! Get a reference to the current estimated state of the system
139 
140  //! Predict the current state of the system with optional control
141  /*! The predict method will extrapolate the new state of the system from the current estimated state
142  by running FilterModel::predict a few times to try to estimate where your model is heading.
143 
144  \param control An optional FilterModel::ControlType value. This value
145  is just directly forwarded to the FilterModel::predict method
146 
147  \note Calling predict() too many times without a call to update() will
148  cause the filter's covariance to grow, and may make the state()
149  become unstable. */
150  void predict(typename FilterModel::ControlType const & control = typename FilterModel::ControlType());
151 
152  //! Update the filter with an observation
153  /*! The update method is used to incorporate real measurements from the system into the estimated state
154 
155  \param observation A real observation taken from your sensor as a Gaussian PDF. */
156  void update(GaussianPDF<ObservationDefinitionType> const & observation);
157 
158  //! Change the prediction covariance of the model
159  void setPredictionCovariance(Eigen::Matrix<double, itsNS, itsNS> const & predictionCovariance);
160 
161  private:
164  typedef typename FilterModel::ControlType ControlType;
165 
166  std::vector<Eigen::Matrix<double, itsNS, 1>> generateSigmaPoints();
167 
169  double itsGamma;
170  std::vector<double> itsWm;
171  std::vector<double> itsWc;
172  Eigen::Matrix<double, itsNS, itsNS> itsR;
173  };
174 }
175 
177 
178 #endif // INCLUDE_NRT_PROBABILISTIC_BAYESFILTERS_UKF_H