//============================================================================
//
//  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 3.0 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
//  
//  Copyright 2004, The University of Texas at Austin
//
//============================================================================

//============================================================================
//
//This software developed by Applied Research Laboratories at the University of
//Texas at Austin, under contract to an agency or agencies within the U.S. 
//Department of Defense. The U.S. Government retains all rights to use,
//duplicate, distribute, disclose, or release this software. 
//
//Pursuant to DoD Directive 523024 
//
// DISTRIBUTION STATEMENT A: This software has been approved for public 
//                           release, distribution is unlimited.
//
//=============================================================================

/**
 * @file SimpleKalmanFilter.hpp
 * Class to compute the solution using a Kalman filter.
 */

#ifndef SIMPLEKALMANFILTER_HPP
#define SIMPLEKALMANFILTER_HPP

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


namespace gpstk
{

      /// @ingroup GPSsolutions 
      /// @ingroup math

      //@{

      /** This class computes the solution using a Kalman filter.
       *
       * A typical way to use this class follows:
       *
       * @code
       *    // Declarations and initializations here...
       *
       *    SimpleKalmanFilter kalman(xhat0, pmatrix);
       *
       *    while(cin >> x >> y)
       *    {
       *
       *       try
       *       {
       *
       *          meas(0) = x;
       *          meas(1) = y;
       *
       *          kalman.Compute(phimatrix, qmatrix, meas, hmatrix, rmatrix);
       *
       *          cout << kalman.xhat(0) << " " << kalman.xhat(1) << endl;
       *       }
       *       catch (Exception e)
       *       {
       *          cout << e;
       *       }
       *    }
       * @endcode
       *
       * There is another way to use this class:
       * @code
       *       // Declarations and initializations here...
       *    SimpleKalmanFilter kalman(xhat0, pmatrix);
       *
       *       // Call this method to reset the filter
       *    kalman.Reset(initialValue,initialErrorVariance);
       *    
       *       // One time time update
       *    kalman.TimeUpdate(phiMatrix, processNoiseCovariance );
       *
       *       // Following several times measurement update
       *    kalman.MeasUpdate( measurements1, measurementsMatrix1, 
       *                                               measurementsCovariance1);
       *
       *    kalman.MeasUpdate( measurements2, measurementsMatrix2, 
       *                                               measurementsCovariance2);
       *      // Get the final solution
       *    Vector<double> x = kalman.xwhat;
       *    Matrix<double> p = kalman.P;
       *
       * @endcode
       *
       * More information about the Kalman filter may be found in the
       * excellent and easy introduction by Welch, G. and G. Bishop.
       * "An Introduction to the Kalman Filter", at:
       * http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html.
       *
       * However, be aware that the algorithm used here is the modified
       * version presented in G. J. Bierman. "Factorization Methods for
       * Discrete Sequential Estimation". Mathematics in Science and
       * Engineering, Vol. 128. Academic Press, New York, 1977. This version
       * enjoys better numerical stability.
       *
       */
   class SimpleKalmanFilter
   {
   public:

         /// Default constructor.
      SimpleKalmanFilter()
         : xhat(1,0.0), P(1,1,0.0), xhatminus(1,0.0), Pminus(1,1,0.0) {};


         /** Common constructor.
          *
          * @param initialState     Vector setting the initial state of
          *                         the system.
          * @param initialErrorCovariance    Matrix setting the initial
          *                values of the a posteriori error covariance.
          */
      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) {};


         /** Common constructor. This is meant to be used with one-dimensional
          * systems.
          *
          * @param initialValue      Initial value of system state.
          * @param initialErrorVariance  Initial value of the a posteriori
          *                              error variance.
          */
      SimpleKalmanFilter( const double& initialValue,
                          const double& initialErrorVariance )
         : xhat(1,initialValue),
           P(1,1,initialErrorVariance), xhatminus(1,0.0),
           Pminus(1,1,0.0) {};


         /** Reset method.
          *
          * This method will reset the filter, setting new values for initial
          * system state vector and the a posteriori error covariance matrix.
          *
          * @param initialState      Vector setting the initial state of
          *                          the system.
          * @param initialErrorCovariance    Matrix setting the initial
          *                   values of the a posteriori error covariance.
          */
      virtual void Reset( const Vector<double>& initialState,
                          const Matrix<double>& initialErrorCovariance );


         /** Reset method.
          *
          * This method will reset the filter, setting new values for initial
          * system  state and the a posteriori error variance. Used for
          * one-dimensional systems.
          *
          * @param initialValue      Initial value of system state.
          * @param initialErrorVariance  Initial value of the a posteriori
          *                              error variance.
          */
      virtual void Reset( const double& initialValue,
                          const double& initialErrorVariance );


         /** Compute the a posteriori estimate of the system state, as well
          *  as the a posteriori estimate error covariance matrix.
          *
          * @param phiMatrix         State transition matrix.
          * @param controlMatrix     Control matrix.
          * @param controlInput      Control input vector.
          * @param processNoiseCovariance    Process noise covariance matrix.
          * @param measurements      Measurements vector.
          * @param measurementsMatrix    Measurements matrix. Called geometry
          *                              matrix in GNSS.
          * @param measurementsNoiseCovariance   Measurements noise covariance
          *                                      matrix.
          *
          * @return
          *  0 if OK
          *  -1 if problems arose
          */
      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);


         /** Compute the a posteriori estimate of the system state, as well
          *  as the a posteriori estimate error covariance matrix. This
          *  version assumes that no control inputs act on the system.
          *
          * @param phiMatrix         State transition matrix.
          * @param processNoiseCovariance    Process noise covariance matrix.
          * @param measurements      Measurements vector.
          * @param measurementsMatrix    Measurements matrix. Called geometry
          *                              matrix in GNSS.
          * @param measurementsNoiseCovariance   Measurements noise covariance
          *                                      matrix.
          *
          * @return
          *  0 if OK
          *  -1 if problems arose
          */
      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);


         /** Compute the a posteriori estimate of the system state, as well
          *  as the a posteriori estimate error variance. Version for
          *  one-dimensional systems.
          *
          * @param phiValue          State transition gain.
          * @param controlGain       Control gain.
          * @param controlInput      Control input value.
          * @param processNoiseVariance    Process noise variance.
          * @param measurement       Measurement value.
          * @param measurementsGain  Measurements gain.
          * @param measurementsNoiseVariance   Measurements noise variance.
          *
          * @return
          *  0 if OK
          *  -1 if problems arose
          */
      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);


         /** Compute the a posteriori estimate of the system state, as well
          *  as the a posteriori estimate error variance. Version for
          *  one-dimensional systems without control input on the system.
          *
          * @param phiValue          State transition gain.
          * @param processNoiseVariance    Process noise variance.
          * @param measurement       Measurement value.
          * @param measurementsGain  Measurements gain.
          * @param measurementsNoiseVariance   Measurements noise variance.
          *
          * @return
          *  0 if OK
          *  -1 if problems arose
          */
      virtual int Compute( const double& phiValue,
                           const double& processNoiseVariance,
                           const double& measurement,
                           const double& measurementsGain,
                           const double& measurementsNoiseVariance )
         throw(InvalidSolver);


         /** Predicts (or "time updates") the a priori estimate of the
          *  system state, as well as the a priori estimate error
          *  covariance matrix.
          *  This version assumes that the phiMatrix is indedify matrix, 
          *  previousState is the last state of the filter, no process
          *  noise, and no control inputs act on the system.
          *
          * @return
          *  0 if OK
          *  -1 if problems arose
          */
      virtual int TimeUpdate()
      { xhatminus=xhat;Pminus=P; return 0;}


      /** Predicts (or "time updates") the a priori estimate of the
          *  system state, as well as the a priori estimate error
          *  covariance matrix.
          *  This version assumes that no control inputs act on the system.
          *
          * @param phiMatrix         State transition matrix.
          * @param processNoiseCovariance    Process noise covariance matrix.
          *
          * @return
          *  0 if OK
          *  -1 if problems arose
          */
      virtual int TimeUpdate( const Matrix<double>& phiMatrix,
                              const Matrix<double>& processNoiseCovariance )
         throw(InvalidSolver)
      { return Predict(phiMatrix,xhat,processNoiseCovariance); }


         /** Predicts (or "time updates") the a priori estimate of the
          *  system state, as well as the a priori estimate error
          *  covariance matrix.
          *  This version assumes that no control inputs act on the system.
          *
          * @param phiMatrix         State transition matrix.
          * @param previousState     Previous system state vector. It is
          *                          the last computed xhat.
          * @param processNoiseCovariance    Process noise covariance matrix.
          *
          * @return
          *  0 if OK
          *  -1 if problems arose
          */
      virtual int TimeUpdate( const Matrix<double>& phiMatrix,
                              const Vector<double>& previousState,
                              const Matrix<double>& processNoiseCovariance )
         throw(InvalidSolver)
      { return Predict(phiMatrix,previousState,processNoiseCovariance); }



         /** Corrects (or "measurement updates") the a posteriori estimate
          *  of the system state vector, as well as the a posteriori estimate
          *  error covariance matrix, using as input the predicted a priori
          *  state vector and error covariance matrix, plus measurements and
          *  associated matrices.
          *
          * @param measurements      Measurements vector.
          * @param measurementsMatrix    Measurements matrix. Called geometry
          *                              matrix in GNSS.
          * @param measurementsNoiseCovariance   Measurements noise covariance
          *                                      matrix.
          *
          * @return
          *  0 if OK
          *  -1 if problems arose
          */
      virtual int MeasUpdate( const Vector<double>& measurements,
                              const Matrix<double>& measurementsMatrix,
                              const Matrix<double>& measurementsNoiseCovariance)
         throw(InvalidSolver)
      { 
         return Correct(measurements,
                        measurementsMatrix,
                        measurementsNoiseCovariance); 
      }


         /// Destructor.
      virtual ~SimpleKalmanFilter() {};


         /// A posteriori state estimation. This is usually your target.
      Vector<double> xhat;


         /// A posteriori error covariance.
      Matrix<double> P;


         /// A priori state estimation.
      Vector<double> xhatminus;


         /// A priori error covariance.
      Matrix<double> Pminus;


   private:


         /** Predicts (or "time updates") the a priori estimate of the
          *  system state, as well as the a priori estimate error covariance
          *  matrix.
          *
          * @param phiMatrix         State transition matrix.
          * @param previousState     Previous system state vector. It is the
          *                          last computed xhat.
          * @param controlMatrix     Control matrix.
          * @param controlInput      Control input vector.
          * @param processNoiseCovariance    Process noise covariance matrix.
          *
          * @return
          *  0 if OK
          *  -1 if problems arose
          */
      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);


         /** Predicts (or "time updates") the a priori estimate of the
          *  system state, as well as the a priori estimate error variance.
          *  Version for one-dimensional systems.
          *
          * @param phiValue          State transition gain.
          * @param previousState     Previous system state. It is the last
          *                          computed xhat.
          * @param controlGain       Control gain.
          * @param controlInput      Control input value.
          * @param processNoiseVariance    Process noise variance.
          *
          * @return
          *  0 if OK
          *  -1 if problems arose
          */
      virtual int Predict( const double& phiValue,
                           const double& previousState,
                           const double& controlGain,
                           const double& controlInput,
                           const double& processNoiseVariance )
         throw(InvalidSolver);


         /** Predicts (or "time updates") the a priori estimate of the
          *  system state, as well as the a priori estimate error
          *  covariance matrix.
          *  This version assumes that no control inputs act on the system.
          *
          * @param phiMatrix         State transition matrix.
          * @param previousState     Previous system state vector. It is
          *                          the last computed xhat.
          * @param processNoiseCovariance    Process noise covariance matrix.
          *
          * @return
          *  0 if OK
          *  -1 if problems arose
          */
      virtual int Predict( const Matrix<double>& phiMatrix,
                           const Vector<double>& previousState,
                           const Matrix<double>& processNoiseCovariance )
         throw(InvalidSolver);


         /** Predicts (or "time updates") the a priori estimate of the
          *  system state, as well as the a priori estimate error variance.
          *  Version for one-dimensional systems and no control input acting
          *  on the system.
          *
          * @param phiValue          State transition gain.
          * @param previousState     Previous system state. It is the last
          *                          computed xhat.
          * @param processNoiseVariance    Process noise variance.
          *
          * @return
          *  0 if OK
          *  -1 if problems arose
          */
      virtual int Predict( const double& phiValue,
                           const double& previousState,
                           const double& processNoiseVariance )
         throw(InvalidSolver);


         /** Corrects (or "measurement updates") the a posteriori estimate
          *  of the system state vector, as well as the a posteriori estimate
          *  error covariance matrix, using as input the predicted a priori
          *  state vector and error covariance matrix, plus measurements and
          *  associated matrices.
          *
          * @param measurements      Measurements vector.
          * @param measurementsMatrix    Measurements matrix. Called geometry
          *                              matrix in GNSS.
          * @param measurementsNoiseCovariance   Measurements noise covariance
          *                                      matrix.
          *
          * @return
          *  0 if OK
          *  -1 if problems arose
          */
      virtual int Correct( const Vector<double>& measurements,
                           const Matrix<double>& measurementsMatrix,
                           const Matrix<double>& measurementsNoiseCovariance )
         throw(InvalidSolver);


         /** Corrects (or "measurement updates") the a posteriori estimate
          *  of the system state value, as well as the a posteriori estimate
          *  error variance, using as input the predicted a priori state and
          *  error variance values, plus measurement and associated gain and
          *  variance. Version for one-dimensional systems.
          *
          * @param measurement       Measurement value.
          * @param measurementsGain  Measurements gain.
          * @param measurementsNoiseVariance   Measurements noise variance.
          *
          * @return
          *  0 if OK
          *  -1 if problems arose
          */
      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
