iteratedextendedkalmanfilter.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 ac 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 __ITERATED_EXTENDED_KALMAN_FILTER__
00022 #define __ITERATED_EXTENDED_KALMAN_FILTER__
00023 
00024 #include "kalmanfilter.h"
00025 #include "../pdf/conditionalpdf.h"
00026 #include "../pdf/gaussian.h"
00027 #include "innovationCheck.h"
00028 # include <map>
00029 
00030 namespace BFL
00031 {
00040   class IteratedExtendedKalmanFilter : public KalmanFilter
00041     {
00042     protected:
00043       virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
00044                              const MatrixWrapper::ColumnVector& u);
00045       virtual void MeasUpdate(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,
00046                               const MatrixWrapper::ColumnVector& z,
00047                               const MatrixWrapper::ColumnVector& s);
00048 
00049     public:
00056       IteratedExtendedKalmanFilter(Gaussian* prior, unsigned int nr_it=1, InnovationCheck* innov = NULL);
00057 
00059       virtual ~IteratedExtendedKalmanFilter();
00060 
00062       //  For realtime use, this function should be called before calling measUpdate
00063       /*  @param vector containing the dimension of the measurement models which are
00064           going to be used
00065       */
00066       void AllocateMeasModelIExt( const vector<unsigned int>& meas_dimensions);
00067 
00069       //  For realtime use, this function should be called before calling measUpdate
00070       /*  @param dimension of the measurement models which is
00071           going to be used
00072       */
00073       void AllocateMeasModelIExt( const unsigned int& meas_dimensions);
00074 
00075     private:
00077       unsigned int _nr_iterations;
00079       InnovationCheck* _innovationChecker;
00080 
00081       struct MeasUpdateVariablesIExt
00082       {
00083         SymmetricMatrix _R_i;
00084         Matrix          _K_i;
00085         Matrix          _H_i;
00086         ColumnVector    _Z_i;
00087         MeasUpdateVariablesIExt() {};
00088         MeasUpdateVariablesIExt(unsigned int meas_dimension, unsigned int state_dimension):
00089           _R_i(meas_dimension)
00090         , _K_i(state_dimension,meas_dimension)
00091         , _H_i(meas_dimension,state_dimension)
00092         , _Z_i(meas_dimension)
00093          {};
00094       }; //struct
00095 
00096       // variables to avoid allocation on the heap
00097       ColumnVector _x;
00098       ColumnVector _x_i;
00099       ColumnVector _x_i_prev;
00100       ColumnVector _J;
00101       ColumnVector _innovation;
00102       Matrix    _F;
00103       SymmetricMatrix _Q;
00104       SymmetricMatrix _P_Matrix;
00105       Matrix          _S_i;
00106       std::map<unsigned int, MeasUpdateVariablesIExt> _mapMeasUpdateVariablesIExt;
00107       std::map<unsigned int, MeasUpdateVariablesIExt>::iterator _mapMeasUpdateVariablesIExt_it;
00108     };  // class
00109 
00110 } // End namespace BFL
00111 
00112 #endif // __ITERATED_EXTENDED_KALMAN_FILTER__
00113 


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