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 // 00005 // This program is free software; you can redistribute it and/or modify 00006 // it under the terms of the GNU Lesser General Public License as published by 00007 // the Free Software Foundation; either version 2.1 of the License, or 00008 // (at your option) any later version. 00009 // 00010 // This program is distributed in the hope that it will be useful, 00011 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00012 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00013 // GNU Lesser General Public License for more details. 00014 // 00015 // You should have received a copy of the GNU Lesser General Public License 00016 // along with this program; if not, write to the Free Software 00017 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 00018 // 00019 00020 #ifndef __NONMINIMAL_KALMAN_FILTER__ 00021 #define __NONMINIMAL_KALMAN_FILTER__ 00022 00023 #include "kalmanfilter.h" 00024 #include "../pdf/conditionalpdf.h" 00025 #include "../pdf/gaussian.h" 00026 #include "../model/nonlinearanalyticmeasurementmodel_gaussianuncertainty.h" 00027 #include "../model/nonlinearanalyticsystemmodel_gaussianuncertainty.h" 00028 #include "nonminimal_state/linearise.h" 00029 #include "../filter/iteratedextendedkalmanfilter.h" 00030 00031 namespace BFL 00032 { 00033 00034 #define NLSysModel NonLinearAnalyticSystemModelGaussianUncertainty 00035 #define NLMeasModel NonLinearAnalyticMeasurementModelGaussianUncertainty 00036 00037 00047 class NonminimalKalmanFilter : public KalmanFilter 00048 { 00049 public: 00059 NonminimalKalmanFilter(Gaussian* prior, 00060 unsigned int NrIterations, 00061 vector<NLSysModel*> minimalsysmodels, 00062 vector<NLMeasModel*> minimalmeasmodels, 00063 vector<GiNaC::symbol> nonlinearstate = *(new vector<GiNaC::symbol>)); 00064 00066 virtual ~NonminimalKalmanFilter(); 00067 00068 // virtual functions 00069 virtual void SysUpdate(SystemModel<ColumnVector>* const sysmodel, 00070 const ColumnVector& u); 00071 virtual void MeasUpdate(MeasurementModel<ColumnVector,ColumnVector>* const measmodel, 00072 const ColumnVector& z, 00073 const ColumnVector& s); 00074 00075 private: 00076 vector<GiNaC::symbol> MinimalState, NonminimalState; 00077 Linearise *Linear; 00078 IteratedExtendedKalmanFilter *NonminimalFilter, *MinimalFilter; 00079 Gaussian *NonminimalPrior, *MinimalPrior; 00080 NLMeasModel *MinimalMeasModel; 00081 00082 }; // class 00083 00084 } // End namespace 00085 00086 #endif // __NONMINIMAL_KALMAN_FILTER__