UKF.H

Go to the documentation of this file.

/*! @file
    @author Randolph Voorhies (voorhies at usc dot edu)
    @copyright GNU Public License (GPL v3)
    @section License
    @verbatim
    // ////////////////////////////////////////////////////////////////////////
    //              The iLab Neuromorphic Robotics Toolkit (NRT)             //
    // Copyright 2010-2012 by the University of Southern California (USC)    //
    //                          and the iLab at USC.                         //
    //                                                                       //
    //                iLab - University of Southern California               //
    //                Hedco Neurociences Building, Room HNB-10               //
    //                    Los Angeles, Ca 90089-2520 - USA                   //
    //                                                                       //
    //      See http://ilab.usc.edu for information about this project.      //
    // ////////////////////////////////////////////////////////////////////////
    // This file is part of The iLab Neuromorphic Robotics Toolkit.          //
    //                                                                       //
    // The iLab Neuromorphic Robotics Toolkit is free software: you can      //
    // redistribute it and/or modify it under the terms of the GNU General   //
    // Public License as published by the Free Software Foundation, either   //
    // version 3 of the License, or (at your option) any later version.      //
    //                                                                       //
    // The iLab Neuromorphic Robotics Toolkit is distributed in the hope     //
    // that it will be useful, but WITHOUT ANY WARRANTY; without even the    //
    // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR       //
    // PURPOSE.  See the GNU General Public License for more details.        //
    //                                                                       //
    // You should have received a copy of the GNU General Public License     //
    // along with The iLab Neuromorphic Robotics Toolkit.  If not, see       //
    // <http://www.gnu.org/licenses/>.                                       //
    // ////////////////////////////////////////////////////////////////////////
    @endverbatim */


#ifndef INCLUDE_NRT_PROBABILISTIC_BAYESFILTERS_UKF_H
#define INCLUDE_NRT_PROBABILISTIC_BAYESFILTERS_UKF_H

#include <nrt/Eigen/Eigen.H>

#include <nrt/Probabilistic/Types/GaussianPDF.H>
#include <nrt/Probabilistic/Types/StateVector.H>
#include <Eigen/Cholesky>

namespace nrt
{
  //! An Unscented Kalman Filter
  /*! The Unscented Kalman Filter is a very general and robust filter used for
      prediction and tracking of potentially non-linear states. The
      implementation here requires only that you provide definitions of the
      state and the observation, as well as a filter model containing two
      functions.

      \tparam StateDefinitionType An nrt::StateDefinition defining the fields
      of the target to be tracked. For example, this State Definition may
      include the current position, velocity and acceleration of a tracked
      vehicle.

      \tparam ObservationDefinitionType An nrt::StateDefinition defining the
      fields that your sensor will return. For example, if you are tracking
      vehicles with a radar system, this State Definition may include the
      measured range and bearing from the radar sensor to the target.

      \tparam FilterModel A simple struct which defines how your tracked target changes over time,
      and how your sensor observes it. Read on for more information.

      \par The Filter Model

      The FilterModel template parameter should be a struct which you define to
      tell the UKF how your target interacts with the world, and how the world
      senses your target. The FilterModel that you provide must have the following definition:

@code
// In this example, we model our target as a vehicle with x,y,theta position
// and translational,angular velocity
NRT_CREATE_STATE_FIELD_GROUP(Position, (x)(y)(theta));
NRT_CREATE_STATE_FIELD_GROUP(Velocity, (translational)(angular));
typedef nrt::StateDefinition<Position, Velocity> VehicleState;

// ... And, we observe our vehicle with a range and angle
NRT_CREATE_STATE_FIELD_GROUP(PolarPosition, (range)(angle));
typedef nrt::StateDefinition<PolarPosition> VehicleObservation;

struct MyCoolFilterModel
{

  // This can be any kind of custom data that your predict method may need to
  // help it make predictions.  For example, if you know the positions of the
  // gas and brake pedals in the tracked vehicle (which would certainly help
  // with the tracking!), then your ControlType struct could contain these
  // values.
  struct ControlType;

  // A function which takes as arguments a state of your target from the last timestep, and a control struct,
  // and must return the predicted position for the current timestep. In our example, we should integrate the
  // Velocity::translational and Velocity::angular into the Position::x, Position::y, and Position::theta
  static nrt::StateVector<VehicleState> 
    predict(nrt::StateVector<VehicleState> const & state, ControlType const & control);

  // A function which takes as an argument a state of the target, and must return what you expect your sensor would retrun
  // if your target was in this state.  In our example, we should return the PolarPosition::range and PolarPosition::angle 
  // from the given state to the radar station.
  static nrt::StateVector<VehicleObservation> 
    observe(nrt::StateVector<VehicleState> const & state)
};
@endcode

      \note The Unscented Kalman Filter works by running the predict and observe
      methods for a few well-chosen points around the current believed state in
      order to estimate how the state is moving. Because of this, you should
      make sure that your predict and observe methods are completely
      re-entrant.

      The neat thing about the UKF is that it can automatically predict
      portions of your state which are not directly observable by your sensor.
      In our vehicle tracking example above, notice that the sensor cannot
      measure the speed or orientation of the vehicle - only the position.
      However, the filter will quickly start estimating the orientation and
      velocity fields automatically after a few timesteps.

      \ingroup probabalistic
      \see probabalistic
  */
  template <class StateDefinitionType, class ObservationDefinitionType, class FilterModel>
  class UKF
  {
    private:
      const static size_t itsNS = StateDefinitionType::num_fields;
      const static size_t itsNO = ObservationDefinitionType::num_fields;

    public:
      //! Construct an Unscented Kalman Filter 
      UKF(GaussianPDF<StateDefinitionType> const & initialState,
          Eigen::Matrix<double, itsNS, itsNS> const & predictionCovariance,
          double alpha = 1e-3, double k = 0, double beta = 2.0);

      //! Get a reference to the current estimated state of the system
      GaussianPDF<StateDefinitionType> & state();

      //! Predict the current state of the system with optional control
      /*! The predict method will extrapolate the new state of the system from the current estimated state
          by running FilterModel::predict a few times to try to estimate where your model is heading. 

          \param control An optional FilterModel::ControlType value. This value
          is just directly forwarded to the FilterModel::predict method

          \note Calling predict() too many times without a call to update() will
          cause the filter's covariance to grow, and may make the state()
          become unstable. */
      void predict(typename FilterModel::ControlType const & control = typename FilterModel::ControlType());

      //! Update the filter with an observation
      /*! The update method is used to incorporate real measurements from the system into the estimated state 

          \param observation A real observation taken from your sensor as a Gaussian PDF. */
      void update(GaussianPDF<ObservationDefinitionType> const & observation);

      //! Change the prediction covariance of the model
      void setPredictionCovariance(Eigen::Matrix<double, itsNS, itsNS> const & predictionCovariance);

    private:
      typedef GaussianPDF<ObservationDefinitionType> ObservationPDFType;
      typedef StateVector<StateDefinitionType> StateVectorType;
      typedef typename FilterModel::ControlType ControlType;

      std::vector<Eigen::Matrix<double, itsNS, 1>> generateSigmaPoints();

      GaussianPDF<StateDefinitionType> itsState;
      double itsGamma;
      std::vector<double> itsWm;
      std::vector<double> itsWc;
      Eigen::Matrix<double, itsNS, itsNS> itsR;
  };
}

#include <nrt/Probabilistic/BayesFilters/details/UKFImpl.H>

#endif // INCLUDE_NRT_PROBABILISTIC_BAYESFILTERS_UKF_H