SMDexample.cpp

Go to the documentation of this file.

/************************************************************************************************/
/*! \file SMDexample.cpp
*  \brief An example implementation of a linear Kalman filter of a spring-mass system using the KalmanFilter class.
*  \author $Author: simpliciter $
*  \version $Revision: 1.1 $
*  \date    $Date: 2003/07/04 16:19:43 $
************************************************************************************************/
/*! 
*
************************************************************************************************/

#include "KalmanFilter.h"
#include "KalmanFilterHistory.h"
#include <iostream>
#include <fstream>
using namespace std;

Matrix SMD_Fmatrix(KalmanFilter*);
Matrix SMD_Hmatrix(KalmanFilter*);
Matrix SMD_Qmatrix(KalmanFilter*);
Matrix SMD_Rmatrix(KalmanFilter*);

int main() {
        
        /*! Declare and initialize intermediate variables */
        Vector x(2);
        Vector z(2);
        Vector zeroVector(2);
        double tLast = 0.0, t = 0.0;
        
        /*! Set intial conditions */
        x(1) = 1.0;
        
        /*! Prepare input file */
        ifstream iFile;
        ofstream oFile;
        
        /*! Open input and output files */
        iFile.open("smdmeasurements.txt");
        oFile.open("smdresults.txt");
        
        /*! Read in initial measurements */
        double z1, z2;
        iFile >> t >> z1 >> z2;
        z(1) = z1; z(2) = z2;
        
        /*! Set up intial kalman filter data */
        Matrix P(2,2);
        P(1,1) = 0.001; P(1,2) = 0.0;
        P(2,1) = 0.0;  P(2,2) = 0.001;
        Matrix K(2,2);
        K(1,1) = 1.0; K(1,2) = 0.0;
        K(2,1) = 0.0; K(2,2) = 1.0;

        /*! Declare and initalize filter and its history */
        KalmanFilter kf(P,K);
        KalmanFilterHistory myHist(&kf);
        
        
        /*! Set up intial sequential filter data */
        Vector states = x;
        Vector controls = zeroVector;
        Vector parameters = zeroVector;
        Vector measurements = z;
        
        kf.SetTimeOfEstimate(tLast);
        kf.SetTimeOfMeasurements(t);
        kf.SetStateVector(states);
        kf.SetControlVector(controls);
        kf.SetMeasurementVector(measurements);
        kf.SetParameterVector(parameters);

        kf.SetSystemProcessNoiseMatrix(&SMD_Qmatrix);
        kf.SetMeasurementCovarianceMatrix(&SMD_Rmatrix);
        kf.SetStateJacobianMatrix(&SMD_Fmatrix);
        kf.SetMeasurementJacobianMatrix(&SMD_Hmatrix);
        
        /*! Output results to output file */
        oFile << kf.GetStateVector() << endl;
        
        while (iFile) {
                
                /*! Get next measurement */
                iFile >> t >> z1 >> z2;
                z(1) = z1; z(2) = z2;
                
                /*! Set new SF data including new measurements */
                measurements = z;
                kf.SetTimeOfMeasurements(t);
                kf.SetMeasurementVector(measurements);
                        
                /*! Estimate state using measurements */
                kf.EstimateState();
                
                /*! Note time of last update */
                tLast = t;
                
                /*! Output results to output file */
                oFile << kf.GetStateVector() << endl;
                
                // write to the history
                myHist.AppendHistory(tLast);
                
        }
                
        Matrix stateData;
        Matrix kalmanData;
        
        myHist.GetKalmanHistory(stateData, kalmanData);
        
        cout << "Here comes the history:" << endl << stateData;
        char pause;
        cout << "Press the any key." << endl;
        cin >> pause;
        
        cout << "Here's the rest:" << endl << kalmanData;
        
        iFile.close();
        oFile.close();
        
        return 1;
        
}

Matrix SMD_Fmatrix(KalmanFilter* kf) {
        
        Matrix temp(2,2);
        double k = 0.2, b = 0.1, m = 5.0;
        
        temp(1,1) = 0.0;    temp(1,2) = 1.0;
        temp(2,1) = -k / m; temp(2,2) = -b / m;
        
        return temp;
        
};

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

Matrix SMD_Qmatrix(KalmanFilter* kf) {
        
        Matrix temp(2,2);
        
        temp(1,1) = 0.001; temp(1,2) = 0.0;
        temp(2,1) = 0.0; temp(2,2) = 0.001;

        return temp;

};

Matrix SMD_Rmatrix(KalmanFilter* kf) {
        
        Matrix temp(2,2);
        double x1_err = 0.2, x2_err = 0.02, x1_std, x2_std;
        
        x1_std = x1_err / pow(2.0, 0.5);
        x2_std = x2_err / pow(2.0, 0.5);
        
        temp(1,1) = pow(x1_std, 2.0); temp(1,2) = 0.0;
        temp(2,1) = 0.0;              temp(2,2) = pow(x1_std, 2.0);
        
        return temp;
        
};

// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
* $Log: SMDexample.cpp,v $
* Revision 1.1  2003/07/04 16:19:43  simpliciter
* Moved.
*
* Revision 1.5  2003/06/28 00:44:58  simpliciter
* Updated for completed KalmanHistory.
*
* Revision 1.4  2003/06/27 01:32:21  simpliciter
* Added in history calls.
*
* Revision 1.3  2003/06/24 23:05:57  mavandyk
* Corrected time handling to reflect changes in the filter classes.
*
* Revision 1.2  2003/06/24 16:02:00  simpliciter
* Modified to not call structs.
*
* Revision 1.1  2003/06/23 21:57:57  mavandyk
* A spring-mass-damper example estimation problem.
*
* Revision 1.4  2003/06/20 22:01:39  mavandyk
* Changes made to reflect the altering of the kalmanFitlerData struct type and the KalmanFilter class, and initialization of the data used in the structs and classes was added.
*
* Revision 1.3  2003/06/18 17:42:20  mavandyk
* Changed the appropriate function input types to reflect changes in KalmanFilter.h.
*
* Revision 1.2  2003/06/12 22:59:26  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.
*
*
******************************************************************************/

---