Bayesian Filtering Library
Generated from SVN r
|
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