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 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__