extendedkalmanfilter.h
Go to the documentation of this file.
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 private:
00070   struct MeasUpdateVariablesExt
00071   {
00072     SymmetricMatrix _R;
00073     Matrix _H;
00074     ColumnVector _Z;
00075     MeasUpdateVariablesExt() {};
00076     MeasUpdateVariablesExt(unsigned int meas_dimension, unsigned int state_dimension):
00077       _R(meas_dimension)
00078     , _H(meas_dimension,state_dimension)
00079     , _Z(meas_dimension)
00080 {};
00081   }; //struct
00082 
00083  protected:
00084   virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
00085                          const MatrixWrapper::ColumnVector& u);
00086   virtual void MeasUpdate(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,
00087                           const MatrixWrapper::ColumnVector& z,
00088                           const MatrixWrapper::ColumnVector& s);
00089   // variables to avoid allocation on the heap
00090   ColumnVector _x;
00091   ColumnVector _J;
00092   Matrix    _F;
00093   SymmetricMatrix _Q;
00094   std::map<unsigned int, MeasUpdateVariablesExt> _mapMeasUpdateVariablesExt;
00095   std::map<unsigned int, MeasUpdateVariablesExt>::iterator _mapMeasUpdateVariablesExt_it;
00096 
00097 
00098 };  // class
00099 
00100 } // End namespace BFL
00101 
00102 #endif // __EXTENDED_KALMAN_FILTER__


bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Mon Feb 11 2019 03:45:12