Bayesian Filtering Library Generated from SVN r
|
00001 // $Id: kalmanfilter.h 30022 2009-03-06 15:10:30Z 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 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 __KALMAN_FILTER__ 00022 #define __KALMAN_FILTER__ 00023 00024 #include "filter.h" 00025 #include "../pdf/gaussian.h" 00026 #include "../model/analyticmeasurementmodel_gaussianuncertainty.h" 00027 #include "../model/analyticsystemmodel_gaussianuncertainty.h" 00028 # include <map> 00029 00030 namespace BFL 00031 { 00032 00034 00049 class KalmanFilter : public Filter<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector> 00050 { 00051 public: 00053 00056 KalmanFilter(Gaussian* prior); 00057 00059 virtual ~KalmanFilter(); 00060 00061 // implement virtual function 00062 virtual Gaussian* PostGet(); 00063 00065 // For realtime use, this function should be called before calling measUpdate 00066 /* @param vector containing the dimension of the measurement models which are 00067 going to be used 00068 */ 00069 void AllocateMeasModel( const vector<unsigned int>& meas_dimensions); 00070 00072 // For realtime use, this function should be called before calling measUpdate 00073 /* @param dimension of the measurement models which is 00074 going to be used 00075 */ 00076 void AllocateMeasModel( const unsigned int& meas_dimensions); 00077 00078 private: 00079 struct MeasUpdateVariables 00080 { 00081 Matrix _S_Matrix; 00082 Matrix _K; 00083 ColumnVector _innov; 00084 Matrix _postHT; 00085 MeasUpdateVariables() {}; 00086 MeasUpdateVariables(unsigned int meas_dimension, unsigned int state_dimension): 00087 _S_Matrix(meas_dimension,meas_dimension) 00088 , _K(state_dimension,meas_dimension) 00089 , _innov(meas_dimension) 00090 , _postHT(state_dimension,meas_dimension) 00091 {}; 00092 }; //struct 00093 00094 protected: 00095 // variables to avoid allocation during update calls 00096 ColumnVector _Mu_new; 00097 SymmetricMatrix _Sigma_new; 00098 Matrix _Sigma_temp; 00099 Matrix _Sigma_temp_par; 00100 Matrix _SMatrix; 00101 Matrix _K; 00102 std::map<unsigned int, MeasUpdateVariables> _mapMeasUpdateVariables; 00103 std::map<unsigned int, MeasUpdateVariables>::iterator _mapMeasUpdateVariables_it; 00104 00105 00110 friend class NonminimalKalmanFilter; 00111 00113 void PostSigmaSet( const MatrixWrapper::SymmetricMatrix& s); 00114 00116 void PostMuSet( const MatrixWrapper::ColumnVector& c); 00117 00122 void CalculateSysUpdate(const MatrixWrapper::ColumnVector& J, const MatrixWrapper::Matrix& F, const MatrixWrapper::SymmetricMatrix& Q); 00123 00130 void CalculateMeasUpdate(const MatrixWrapper::ColumnVector& z, const MatrixWrapper::ColumnVector& Z, const MatrixWrapper::Matrix& H, const MatrixWrapper::SymmetricMatrix& R); 00131 00133 00138 virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel, 00139 const MatrixWrapper::ColumnVector& u) = 0; 00140 00142 00153 virtual void MeasUpdate(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel, 00154 const MatrixWrapper::ColumnVector& z, 00155 const MatrixWrapper::ColumnVector& s) = 0; 00156 00157 virtual bool UpdateInternal(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel, 00158 const MatrixWrapper::ColumnVector& u, 00159 MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel, 00160 const MatrixWrapper::ColumnVector& z, 00161 const MatrixWrapper::ColumnVector& s); 00162 00163 }; // class 00164 00165 00166 00167 } // End namespace BFL 00168 00169 #endif // __KALMAN_FILTER__