simplekalmanfilter.hpp

一个gps小工具包

          *  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);


         /// 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);


   }; // class SimpleKalmanFilter


      //@}

} // namespace
#endif // SIMPLEKALMANFILTER_HPP