Bayesian Filtering Library  Generated from SVN r
extendedkalmanfilter.h
00001 // $Id$
00002 // Copyright (C) 2002 Klaas Gadeyne <first dot last at gmail dot com>
00003 //                    Wim Meeussen  <wim dot meeussen at mech dot kuleuven dot be>
00004 //                    Tinne De Laet <tinne dot delaet at mech dot kuleuven dot be>
00005 //
00006 // This program is free software; you can redistribute it and/or modify
00007 // it under the terms of the GNU Lesser General Public License as published by
00008 // the Free Software Foundation; either version 2.1 of the License, or
00009 // (at your option) any later version.
00010 //
00011 // This program is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU Lesser General Public License for more details.
00015 //
00016 // You should have received a copy of the GNU Lesser General Public License
00017 // along with this program; if not, write to the Free Software
00018 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00019 //
00020 
00021 #ifndef __EXTENDED_KALMAN_FILTER__
00022 #define __EXTENDED_KALMAN_FILTER__
00023 
00024 #include "kalmanfilter.h"
00025 #include "../pdf/conditionalpdf.h"
00026 #include "../pdf/gaussian.h"
00027 # include <map>
00028 
00029 namespace BFL
00030 {
00031 
00043 class ExtendedKalmanFilter : public KalmanFilter
00044 {
00045 public:
00050   ExtendedKalmanFilter(Gaussian* prior);
00051 
00053   virtual ~ExtendedKalmanFilter();
00054 
00056   //  For realtime use, this function should be called before calling measUpdate
00057   /*  @param vector containing the dimension of the measurement models which are
00058       going to be used
00059   */
00060   void AllocateMeasModelExt( const vector<unsigned int>& meas_dimensions);
00061 
00063   //  For realtime use, this function should be called before calling measUpdate
00064   /*  @param dimension of the measurement models which is
00065       going to be used
00066   */
00067   void AllocateMeasModelExt( const unsigned int& meas_dimensions);
00068 
00069   // Get NIS value
00070   double NisGet(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel, const MatrixWrapper::ColumnVector& z);
00071 
00072 private:
00073   struct MeasUpdateVariablesExt
00074   {
00075     SymmetricMatrix _R;
00076     Matrix _H;
00077     ColumnVector _Z;
00078     MeasUpdateVariablesExt() {};
00079     MeasUpdateVariablesExt(unsigned int meas_dimension, unsigned int state_dimension):
00080       _R(meas_dimension)
00081     , _H(meas_dimension,state_dimension)
00082     , _Z(meas_dimension)
00083 {};
00084   }; //struct
00085 
00086  protected:
00087   virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
00088                          const MatrixWrapper::ColumnVector& u);
00089   virtual void MeasUpdate(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,
00090                           const MatrixWrapper::ColumnVector& z,
00091                           const MatrixWrapper::ColumnVector& s);
00092   // variables to avoid allocation on the heap
00093   ColumnVector _x;
00094   ColumnVector _J;
00095   Matrix    _F;
00096   SymmetricMatrix _Q;
00097   std::map<unsigned int, MeasUpdateVariablesExt> _mapMeasUpdateVariablesExt;
00098   std::map<unsigned int, MeasUpdateVariablesExt>::iterator _mapMeasUpdateVariablesExt_it;
00099 
00100 
00101 };  // class
00102 
00103 } // End namespace BFL
00104 
00105 #endif // __EXTENDED_KALMAN_FILTER__