KalmanFilter.cpp

Go to the documentation of this file.

/************************************************************************************************/
/************************************************************************************************/
/*! \file KalmanFilter.cpp
*  \brief Definitions of the member functions of the KalmanFilter class.
*  \author $Author: simpliciter $
*  \version $Revision: 1.15 $
*  \date    $Date: 2003/07/14 19:38:15 $
************************************************************************************************/
/*! 
*
************************************************************************************************/

#include "KalmanFilter.h"
#include "LKFfunctions.h"
#include <iostream>



// Contructors/Deconstructors
KalmanFilter::KalmanFilter() {
        
        SetSystemProcessNoiseMatrix(&Qmatrix);
        SetMeasurementCovarianceMatrix(&Rmatrix);
        SetStateJacobianMatrix(&Fmatrix);
        SetMeasurementJacobianMatrix(&Hmatrix);

        SetStatePropagator(&LKFStatePropagator);
    SetCovariancePropagator(&LKFCovarianceMatrixPropagator);
    SetKalmanGainCalculation(&LKFCalcualteKalmanGain);
    SetStateMeasurementUpdate(&LKFStateMeasurementUpdate);
    SetCovarianceMeasurementUpdate(&LKFCovarianceMeasurementUpdate);
        
};

KalmanFilter::KalmanFilter(Matrix& _covariance, Matrix& _kalmanGain) {
        
        SetCovarianceMatrix(_covariance);
        SetKalmanGainMatrix(_kalmanGain);
        
        SetSystemProcessNoiseMatrix(&Qmatrix);
        SetMeasurementCovarianceMatrix(&Rmatrix);
        SetStateJacobianMatrix(&Fmatrix);
        SetMeasurementJacobianMatrix(&Hmatrix);
        
        SetStatePropagator(&LKFStatePropagator);
    SetCovariancePropagator(&LKFCovarianceMatrixPropagator);
    SetKalmanGainCalculation(&LKFCalcualteKalmanGain);
    SetStateMeasurementUpdate(&LKFStateMeasurementUpdate);
    SetCovarianceMeasurementUpdate(&LKFCovarianceMeasurementUpdate);
        
};

KalmanFilter::~KalmanFilter() { };

// Public member functions
Matrix KalmanFilter::GetCovarianceMatrix() { return m_covariance; }
Matrix KalmanFilter::GetKalmanGainMatrix() { return m_kalmanGain; }

Matrix KalmanFilter::GetSystemProcessNoiseMatrix() { return (*ptr_systemProcessNoiseMatrix)(this); }
Matrix KalmanFilter::GetMeasurementCovarianceMatrix() { return (*ptr_measurementCovarianceMatrix)(this); }
Matrix KalmanFilter::GetStateJacobianMatrix() { return (*ptr_stateJacobianMatrix)(this); }
Matrix KalmanFilter::GetMeasurementJacobianMatrix() { return (*ptr_measurementJacobianMatrix)(this); }

void KalmanFilter::EstimateState() {
        
        PropagateState();       
        PropagateCovariance();
        CalculateKalmanGain();
        MeasurementUpdateState();
        MeasurementUpdateCovariance();
        m_timeOfEstimate = m_timeOfMeasurements;
        
};

// Public member functions
void KalmanFilter::PropagateState() { ptr_statePropagteFunc(this); };
void KalmanFilter::PropagateCovariance() { ptr_covariancePropagateFunc(this); };
void KalmanFilter::CalculateKalmanGain() { ptr_kalmanGainCalculationFunc(this); };
void KalmanFilter::MeasurementUpdateState() { ptr_stateMeasurementUpdateFunc(this); };
void KalmanFilter::MeasurementUpdateCovariance() { ptr_covarianceMeasurementUpdateFunc(this); };

// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
* $Log: KalmanFilter.cpp,v $
* Revision 1.15  2003/07/14 19:38:15  simpliciter
* Fixed typo
*
* Revision 1.14  2003/06/27 01:49:46  simpliciter
* Got a bit "commit" happy.  No changes.
*
*
* Revision 1.12  2003/06/24 23:05:01  mavandyk
* Added a time update statement to the estimate state function.
*
* Revision 1.11  2003/06/24 22:40:24  mavandyk
* Moved inline function definition from implementation to header file.
*
* Revision 1.10  2003/06/24 15:40:31  simpliciter
* In brief:  I removed the kfData struct.
* That change required numerous other changes.
* Most significant is that functions which previously
* took kfData or sfData as an input now take a KalmanFilter*.
*
* Revision 1.9  2003/06/20 21:59:24  mavandyk
* Pulled pointer types from kalmanFilterData struct and put them directly in the KalmanFilter class.
*
* Revision 1.8  2003/06/20 17:54:57  simpliciter
* Fixed what I broke--need to reinstate inline functions.
*
* Revision 1.6  2003/06/20 15:21:27  simpliciter
* Added deconstructor.
*
* Revision 1.5  2003/06/19 23:29:10  mavandyk
* Removed an extra parentheses in a function prototype.
*
* Revision 1.4  2003/06/18 17:39:18  mavandyk
* Added include lines for KalmanFilter.h and LKFfunctions.h changed the function pointers, and added KalmanFilter:: to each of the function definitions.
*
* Revision 1.3  2003/06/12 23:07:45  mavandyk
* Added footer comment log.
*
* Revision 1.5  2003/06/12 16:10:36  mavandyk
* Changed all references to iterative filter to sequential filter for greater clarity.
*
* Revision 1.4  2003/06/11 17:52:26  simpliciter
* Made a derived class from IterativeFilter.
*
* Revision 1.3  2003/06/11 15:45:32  simpliciter
* Added IterativeFilter.h include line.
*
* Revision 1.2  2003/06/11 13:07:50  simpliciter
* Minor changes.
*
* Revision 1.1  2003/06/11 03:03:46  simpliciter
* Initial submission.
*
******************************************************************************/

---