Bayesian Filtering Library Generated from SVN r
|
00001 // $Id: extendedkalmanfilter.h 29830 2009-01-14 15:10:41Z kgadeyne $ 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__