LKFfunctions.cpp

Go to the documentation of this file.

/************************************************************************************************/
/*! \file LKFfunctions.cpp
*  \brief Definitions of the functions used by the KalmanFilter constructor.
*  \author $Author: simpliciter $
*  \version $Revision: 1.9 $
*  \date    $Date: 2003/07/14 19:37:22 $
************************************************************************************************/
/*! 
*
************************************************************************************************/

#include "LKFfunctions.h"

void LKFStatePropagator(KalmanFilter* kf) {
        
        // Get sequential filter data
        Vector states, controls, measurements, parameters;
        states = kf->GetStateVector();
        controls = kf->GetControlVector();
        measurements = kf->GetMeasurementVector();
        parameters = kf->GetParameterVector();
        
        // Determine length of state vector
        int lengthOfStateVector;
        lengthOfStateVector = states.getIndexCount();
        
        // Calculate the phi matrix
        Matrix phi; //~ (lengthOfStateVector,lengthOfStateVector);
        phi = eye(lengthOfStateVector) + kf->GetStateJacobianMatrix() * kf->GetTimeStep();  // is this right?
        
        // Propagate the state vector
        states = phi * states;
        
        // Set the new state vector
        kf->SetStateVector(states);
        
};

void LKFCovarianceMatrixPropagator(KalmanFilter* kf) {
        
        // Get sequential filter data
        Vector states, controls, measurements, parameters;
        states = kf->GetStateVector();
        controls = kf->GetControlVector();
        measurements = kf->GetMeasurementVector();
        parameters = kf->GetParameterVector();
        
        // Get Kalman filter data
        Matrix covariance, kalmanGain;
        covariance = kf->GetCovarianceMatrix();
        kalmanGain = kf->GetKalmanGainMatrix();
        
        // Determine length of state vector
        int lengthOfStateVector;
        lengthOfStateVector = states.getIndexCount();
        
        // Calculate the phi matrix
        Matrix phi; //~ (lengthOfStateVector,lengthOfStateVector);
        phi = eye(lengthOfStateVector) + kf->GetStateJacobianMatrix() * kf->GetTimeStep();
        
        // Calculate Qk ??? a utility function to calculate Qk from Q
        Matrix Qk; //~ (lengthOfStateVector,lengthOfStateVector);
        Qk = kf->GetSystemProcessNoiseMatrix();
        
        // Propagate the covariance matrix
        covariance = phi * covariance * ~phi + Qk;
        
        // Set the new covariance matrix
        kf->SetCovarianceMatrix(covariance);
        
};

void LKFCalcualteKalmanGain(KalmanFilter* kf) {
        
        // Get sequential filter data
        /*Vector states, controls, measurements, parameters;
        states = kf->GetStateVector();
        controls = kf->GetControlVector();
        measurements = kf->GetMeasurementVector();
        parameters = kf->GetParameterVector();
        */
        
        // Get Kalman filter data
        Matrix covariance, kalmanGain;
        covariance = kf->GetCovarianceMatrix();
        //kalmanGain = kf->GetKalmanGainMatrix();
        
        // Determine length of state vector
        //~ int lengthOfStateVector;
        //~ lengthOfStateVector = states.getIndexCount();
        
        // Calculate measurement matrix
        Matrix Hk; //~ (lengthOfMeasurementVector,lengthOfStateVector);
        Hk = kf->GetMeasurementJacobianMatrix();
        
        // Calculate the Kalman gains
        kalmanGain = (covariance * ~Hk) * (Hk * covariance * ~Hk + kf->GetMeasurementCovarianceMatrix()).inverse();
        // Set the new Kalman Gain matrix
        kf->SetKalmanGainMatrix(kalmanGain);
        
};

void LKFStateMeasurementUpdate(KalmanFilter* kf) {
        
        // Get sequential filter data
        Vector states, controls, measurements, parameters;
        states = kf->GetStateVector();
        controls = kf->GetControlVector();
        measurements = kf->GetMeasurementVector();
        parameters = kf->GetParameterVector();
        
        // Get Kalman filter data
        Matrix covariance, kalmanGain;
        covariance = kf->GetCovarianceMatrix();
        kalmanGain = kf->GetKalmanGainMatrix();
        
        // Determine length of state vector
        //~ int lengthOfStateVector;
        //~ lengthOfStateVector = states.getIndexCount();
        
        // Calculate measurement matrix
        Matrix Hk;//~ (lengthOfStateVector,lengthOfStateVector);
        Hk = kf->GetMeasurementJacobianMatrix();
        
        // Update state vector
        states = states + kalmanGain * (measurements - kf->GetEstimatedMeasurements());
        
        // Set the new state vector
        kf->SetStateVector(states);
        
};

void LKFCovarianceMeasurementUpdate(KalmanFilter* kf) {
        
        // Get sequential filter data
        Vector states, controls, measurements, parameters;
        states = kf->GetStateVector();
        /*controls = kf->GetControlVector();
        measurements = kf->GetMeasurementVector();
        parameters = kf->GetParameterVector();
        */
        
        // Get Kalman filter data
        Matrix covariance, kalmanGain;
        covariance = kf->GetCovarianceMatrix();
        kalmanGain = kf->GetKalmanGainMatrix();
        
        // Determine length of state vector
        int lengthOfStateVector;
        lengthOfStateVector = states.getIndexCount();
        
        // Calculate measurement matrix
        Matrix Hk; //~ (lengthOfStateVector,lengthOfStateVector);
        Hk = kf->GetMeasurementJacobianMatrix();
        
        // Update covariance matrix
        covariance = (eye(lengthOfStateVector) - kalmanGain * Hk) * covariance;
        
        // Set the new covariance matrix
        kf->SetCovarianceMatrix(covariance);
        
};

Matrix Fmatrix(KalmanFilter* kf) {
        
        Matrix temp(2,2);
        
        temp(1,1) = 0.1; temp(1,2) = 0.0;
        temp(2,1) = 0.0; temp(2,2) = 0.1;
        
        return temp;
        
};

Matrix Hmatrix(KalmanFilter* kf) {
        
        Matrix temp(2,2);
        
        temp(1,1) = 0.1; temp(1,2) = 0.0;
        temp(2,1) = 0.0; temp(2,2) = 0.1;
        
        return temp;
        
};

Matrix Qmatrix(KalmanFilter* kf) {
        
        Matrix temp(2,2);
        
        temp(1,1) = 0.1; temp(1,2) = 0.0;
        temp(2,1) = 0.0; temp(2,2) = 0.1;

        return temp;

};

Matrix Rmatrix(KalmanFilter* kf) {
        
        Matrix temp(2,2);
        
        temp(1,1) = 0.1; temp(1,2) = 0.0;
        temp(2,1) = 0.0; temp(2,2) = 0.1;
        
        return temp;
        
};

// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
* $Log: LKFfunctions.cpp,v $
* Revision 1.9  2003/07/14 19:37:22  simpliciter
* Removed extraneous data calls.
*
* Revision 1.8  2003/07/03 00:35:03  simpliciter
* Eliminitated unnecessary matrix size declarations.
* Fixed Kalman Gain equation:  WAS (stuff + stuff)/(stuff)
* IS (stuff) * (stuff+stuff).inverse()
*
* Revision 1.7  2003/06/30 21:10:10  mavandyk
* Split up the relative time data into two absolute time stamps.
*
* Revision 1.6  2003/06/24 15:46:54  simpliciter
* Functions modified to not use structs;
* most now take a KalmanFilter*.
*
* Revision 1.5  2003/06/20 22:00:39  mavandyk
* Changes made to reflect the altering of the kalmanFitlerData struct type and the KalmanFilter class.
*
* Revision 1.4  2003/06/19 23:13:08  mavandyk
* Moved asterish from left to right side of the KalmanFilter types being passed into functions.
*
* Revision 1.3  2003/06/18 17:41:32  mavandyk
* Added include lines for LKFfunctions.h changed the appropriate function pointer input types to reflect changes in KalmanFilter.h.
*
* Revision 1.2  2003/06/12 23:01:41  mavandyk
* Added header and footer comments.
*
* Revision 1.1  2003/06/12 16:29:41  mavandyk
* This file replaces IterativeFilter.h, and only differs by replacing iterative with sequential.
*
* Revision 1.1  2003/06/11 15:45:08  simpliciter
* Initial submission, pulled out of KalmanFilter.h.
* May require (desire) additional pure virtual functions.
*
*
******************************************************************************/

---