SimpleKalmanFilter.hpp

Go to the documentation of this file.

#pragma ident "$Id: SimpleKalmanFilter.hpp 2570 2011-04-21 17:33:49Z yanweignss $"

#ifndef SIMPLEKALMANFILTER_HPP
#define SIMPLEKALMANFILTER_HPP

//============================================================================
//
//  This file is part of GPSTk, the GPS Toolkit.
//
//  The GPSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 2.1 of the License, or
//  any later version.
//
//  The GPSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GPSTk; if not, write to the Free Software Foundation,
//  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//
//  Dagoberto Salazar - gAGE ( http://www.gage.es ). 2007, 2008
//
//============================================================================



#include "Exception.hpp"
#include "Matrix.hpp"
#include "Vector.hpp"
#include "SolverBase.hpp"


namespace gpstk
{




   class SimpleKalmanFilter
   {
   public:

      SimpleKalmanFilter()
         : xhat(1,0.0), P(1,1,0.0), xhatminus(1,0.0), Pminus(1,1,0.0) {};


      SimpleKalmanFilter( const Vector<double>& initialState,
                          const Matrix<double>& initialErrorCovariance )
         : xhat(initialState), P(initialErrorCovariance),
           xhatminus(initialState.size(), 0.0),
           Pminus( initialErrorCovariance.rows(),
                   initialErrorCovariance.cols(), 0.0) {};


      SimpleKalmanFilter( const double& initialValue,
                          const double& initialErrorVariance )
         : xhat(1,initialValue),
           P(1,1,initialErrorVariance), xhatminus(1,0.0),
           Pminus(1,1,0.0) {};


      virtual void Reset( const Vector<double>& initialState,
                          const Matrix<double>& initialErrorCovariance );


      virtual void Reset( const double& initialValue,
                          const double& initialErrorVariance );


      virtual int Compute( const Matrix<double>& phiMatrix,
                           const Matrix<double>& controlMatrix,
                           const Vector<double>& controlInput,
                           const Matrix<double>& processNoiseCovariance,
                           const Vector<double>& measurements,
                           const Matrix<double>& measurementsMatrix,
                           const Matrix<double>& measurementsNoiseCovariance )
         throw(InvalidSolver);


      virtual int Compute( const Matrix<double>& phiMatrix,
                           const Matrix<double>& processNoiseCovariance,
                           const Vector<double>& measurements,
                           const Matrix<double>& measurementsMatrix,
                           const Matrix<double>& measurementsNoiseCovariance )
         throw(InvalidSolver);


      virtual int Compute( const double& phiValue,
                           const double& controlGain,
                           const double& controlInput,
                           const double& processNoiseVariance,
                           const double& measurement,
                           const double& measurementsGain,
                           const double& measurementsNoiseVariance )
         throw(InvalidSolver);


      virtual int Compute( const double& phiValue,
                           const double& processNoiseVariance,
                           const double& measurement,
                           const double& measurementsGain,
                           const double& measurementsNoiseVariance )
         throw(InvalidSolver);


      virtual int TimeUpdate()
      { xhatminus=xhat;Pminus=P; return 0;}


      virtual int TimeUpdate( const Matrix<double>& phiMatrix,
                              const Matrix<double>& processNoiseCovariance )
         throw(InvalidSolver)
      { return Predict(phiMatrix,xhat,processNoiseCovariance); }


      virtual int TimeUpdate( const Matrix<double>& phiMatrix,
                              const Vector<double>& previousState,
                              const Matrix<double>& processNoiseCovariance )
         throw(InvalidSolver)
      { return Predict(phiMatrix,previousState,processNoiseCovariance); }



      virtual int MeasUpdate( const Vector<double>& measurements,
                              const Matrix<double>& measurementsMatrix,
                              const Matrix<double>& measurementsNoiseCovariance)
         throw(InvalidSolver)
      { 
         return Correct(measurements,
                        measurementsMatrix,
                        measurementsNoiseCovariance); 
      }


      virtual ~SimpleKalmanFilter() {};


      Vector<double> xhat;


      Matrix<double> P;


      Vector<double> xhatminus;


      Matrix<double> Pminus;


   private:


      virtual int Predict( const Matrix<double>& phiMatrix,
                           const Vector<double>& previousState,
                           const Matrix<double>& controlMatrix,
                           const Vector<double>& controlInput,
                           const Matrix<double>& processNoiseCovariance )
         throw(InvalidSolver);


      virtual int Predict( const double& phiValue,
                           const double& previousState,
                           const double& controlGain,
                           const double& controlInput,
                           const double& processNoiseVariance )
         throw(InvalidSolver);


      virtual int Predict( const Matrix<double>& phiMatrix,
                           const Vector<double>& previousState,
                           const Matrix<double>& processNoiseCovariance )
         throw(InvalidSolver);


      virtual int Predict( const double& phiValue,
                           const double& previousState,
                           const double& processNoiseVariance )
         throw(InvalidSolver);


      virtual int Correct( const Vector<double>& measurements,
                           const Matrix<double>& measurementsMatrix,
                           const Matrix<double>& measurementsNoiseCovariance )
         throw(InvalidSolver);


      virtual int Correct( const double& measurement,
                           const double& measurementsGain,
                           const double& measurementsNoiseVariance )
         throw(InvalidSolver);


   }; // End of class 'SimpleKalmanFilter'


}  // End of namespace gpstk
#endif // SIMPLEKALMANFILTER_HPP

---