kalmanfilter.h
Go to the documentation of this file.
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__


bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Sun Oct 5 2014 22:29:53