Main Page | Modules | Namespace List | Class Hierarchy | Class List | Directories | File List | Namespace Members | Class Members | File Members | Related Pages | Examples | Search for 

SimpleKalmanFilter Class Reference
[Mathematical algorithms, GPS solution algorithms and Tropospheric]

#include <SimpleKalmanFilter.hpp>

Collaboration diagram for SimpleKalmanFilter:

Collaboration graph
[legend]
List of all members.

Detailed Description

This class computes the solution using a Kalman filter.

A typical way to use this class follows:

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

There is another way to use this class: 

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

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.

Definition at line 113 of file SimpleKalmanFilter.hpp.

Public Member Functions

 SimpleKalmanFilter ()
 Default constructor.
 SimpleKalmanFilter (const Vector< double > &initialState, const Matrix< double > &initialErrorCovariance)
 Common constructor.
 SimpleKalmanFilter (const double &initialValue, const double &initialErrorVariance)
 Common constructor.
virtual void Reset (const Vector< double > &initialState, const Matrix< double > &initialErrorCovariance)
 Reset method.
virtual void Reset (const double &initialValue, const double &initialErrorVariance)
 Reset method.
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.
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 covariance matrix.
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.
virtual int Compute (const double &phiValue, 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.
virtual int TimeUpdate ()
 Predicts (or "time updates") the a priori estimate of the system state, as well as the a priori estimate error covariance matrix.
virtual int TimeUpdate (const Matrix< double > &phiMatrix, 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 covariance matrix.
virtual int TimeUpdate (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 covariance matrix.
virtual int MeasUpdate (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 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.
virtual ~SimpleKalmanFilter ()
 Destructor.

Public Attributes

Vector< double > xhat
 A posteriori state estimation. This is usually your target.
Matrix< double > P
 A posteriori error covariance.
Vector< double > xhatminus
 A priori state estimation.
Matrix< double > Pminus
 A priori error covariance.

Constructor & Destructor Documentation

SimpleKalmanFilter (   )  [inline]
 

Default constructor.

Definition at line 118 of file SimpleKalmanFilter.hpp.

SimpleKalmanFilter (  const Vector< double > &  initialState,
const Matrix< double > &  initialErrorCovariance
)  [inline]
 

Common constructor.

Parameters:
initialState Vector setting the initial state of the system.
initialErrorCovariance Matrix setting the initial values of the a posteriori error covariance.

Definition at line 129 of file SimpleKalmanFilter.hpp.

SimpleKalmanFilter (  const double &  initialValue,
const double &  initialErrorVariance
)  [inline]
 

Common constructor.

This is meant to be used with one-dimensional systems.

Parameters:
initialValue Initial value of system state.
initialErrorVariance Initial value of the a posteriori error variance.

Definition at line 144 of file SimpleKalmanFilter.hpp.

virtual ~SimpleKalmanFilter (   )  [inline, virtual]
 

Destructor.

Definition at line 361 of file SimpleKalmanFilter.hpp.

Member Function Documentation

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

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.

Parameters:
phiValue State transition gain.
processNoiseVariance Process noise variance.
measurement Measurement value.
measurementsGain Measurements gain.
measurementsNoiseVariance Measurements noise variance.
Returns:
0 if OK -1 if problems arose

Definition at line 247 of file SimpleKalmanFilter.cpp.

References GPSTK_THROW.

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]
 

Compute the a posteriori estimate of the system state, as well as the a posteriori estimate error variance.

Version for one-dimensional systems.

Parameters:
phiValue State transition gain.
controlGain Control gain.
controlInput Control input value.
processNoiseVariance Process noise variance.
measurement Measurement value.
measurementsGain Measurements gain.
measurementsNoiseVariance Measurements noise variance.
Returns:
0 if OK -1 if problems arose

Definition at line 199 of file SimpleKalmanFilter.cpp.

References GPSTK_THROW.

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]
 

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.

Parameters:
phiMatrix State transition matrix.
processNoiseCovariance Process noise covariance matrix.
measurements Measurements vector.
measurementsMatrix Measurements matrix. Called geometry matrix in GNSS.
measurementsNoiseCovariance Measurements noise covariance matrix.
Returns:
0 if OK -1 if problems arose

Definition at line 153 of file SimpleKalmanFilter.cpp.

References GPSTK_THROW.

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]
 

Compute the a posteriori estimate of the system state, as well as the a posteriori estimate error covariance matrix.

Parameters:
phiMatrix State transition matrix.
controlMatrix Control matrix.
controlInput Control input vector.
processNoiseCovariance Process noise covariance matrix.
measurements Measurements vector.
measurementsMatrix Measurements matrix. Called geometry matrix in GNSS.
measurementsNoiseCovariance Measurements noise covariance matrix.
Returns:
0 if OK -1 if problems arose

Definition at line 103 of file SimpleKalmanFilter.cpp.

References GPSTK_THROW.

virtual int MeasUpdate (  const Vector< double > &  measurements,
const Matrix< double > &  measurementsMatrix,
const Matrix< double > &  measurementsNoiseCovariance
)  throw (InvalidSolver) [inline, virtual]
 

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.

Parameters:
measurements Measurements vector.
measurementsMatrix Measurements matrix. Called geometry matrix in GNSS.
measurementsNoiseCovariance Measurements noise covariance matrix.
Returns:
0 if OK -1 if problems arose

Definition at line 349 of file SimpleKalmanFilter.hpp.

Referenced by GeneralConstraint::constraintToSolver().

void Reset (  const double &  initialValue,
const double &  initialErrorVariance
)  [virtual]
 

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.

Parameters:
initialValue Initial value of system state.
initialErrorVariance Initial value of the a posteriori error variance.

Definition at line 73 of file SimpleKalmanFilter.cpp.

References SimpleKalmanFilter::P, SimpleKalmanFilter::Pminus, Matrix::resize(), Vector::resize(), SimpleKalmanFilter::xhat, and SimpleKalmanFilter::xhatminus.

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

Reset method.

This method will reset the filter, setting new values for initial system state vector and the a posteriori error covariance matrix.

Parameters:
initialState Vector setting the initial state of the system.
initialErrorCovariance Matrix setting the initial values of the a posteriori error covariance.

Definition at line 49 of file SimpleKalmanFilter.cpp.

References Matrix::cols(), SimpleKalmanFilter::P, SimpleKalmanFilter::Pminus, Matrix::resize(), Vector::resize(), Matrix::rows(), Vector::size(), SimpleKalmanFilter::xhat, and SimpleKalmanFilter::xhatminus.

virtual int TimeUpdate (  const Matrix< double > &  phiMatrix,
const Vector< double > &  previousState,
const Matrix< double > &  processNoiseCovariance
)  throw (InvalidSolver) [inline, virtual]
 

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.

Parameters:
phiMatrix State transition matrix.
previousState Previous system state vector. It is the last computed xhat.
processNoiseCovariance Process noise covariance matrix.
Returns:
0 if OK -1 if problems arose

Definition at line 325 of file SimpleKalmanFilter.hpp.

virtual int TimeUpdate (  const Matrix< double > &  phiMatrix,
const Matrix< double > &  processNoiseCovariance
)  throw (InvalidSolver) [inline, virtual]
 

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.

Parameters:
phiMatrix State transition matrix.
processNoiseCovariance Process noise covariance matrix.
Returns:
0 if OK -1 if problems arose

Definition at line 305 of file SimpleKalmanFilter.hpp.

virtual int TimeUpdate (   )  [inline, virtual]
 

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.

Returns:
0 if OK -1 if problems arose

Definition at line 289 of file SimpleKalmanFilter.hpp.

Member Data Documentation

Matrix<double> P
 

A posteriori error covariance.

Definition at line 369 of file SimpleKalmanFilter.hpp.

Referenced by GeneralConstraint::constraintToSolver(), and SimpleKalmanFilter::Reset().

Matrix<double> Pminus
 

A priori error covariance.

Definition at line 377 of file SimpleKalmanFilter.hpp.

Referenced by SimpleKalmanFilter::Reset().

Vector<double> xhat
 

A posteriori state estimation. This is usually your target.

Definition at line 365 of file SimpleKalmanFilter.hpp.

Referenced by GeneralConstraint::constraintToSolver(), and SimpleKalmanFilter::Reset().

Vector<double> xhatminus
 

A priori state estimation.

Definition at line 373 of file SimpleKalmanFilter.hpp.

Referenced by SimpleKalmanFilter::Reset().

The documentation for this class was generated from the following files:
Generated on Thu Feb 9 03:31:41 2012 for GPS ToolKit Software Library by  doxygen 1.3.9.1