simplekalmanfilter.hpp

gps源代码

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

#ifndef SIMPLE_KALMAN_FILTER_HPP
#define SIMPLE_KALMAN_FILTER_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
//
//============================================================================



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


namespace gpstk
{
    /** @addtogroup 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
       *
       * 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" (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)
        {
            xhat = initialState;
            P = initialErrorCovariance;
            xhatminus.resize(initialState.size(), 0.0);
            Pminus.resize(initialErrorCovariance.rows(), initialErrorCovariance.cols(), 0.0);
        }


        /** 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)
        {
            xhat.resize(1, initialValue);
            P.resize(1,1, initialErrorVariance);
            xhatminus.resize(1, 0.0);
            Pminus.resize(1, 1, 0.0);
        }


        /** 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