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

SMDexample.cpp

Go to the documentation of this file.
00001 /************************************************************************************************/
00002 /*! \file SMDexample.cpp
00003 *  \brief An example implementation of a linear Kalman filter of a spring-mass system using the KalmanFilter class.
00004 *  \author $Author: simpliciter $
00005 *  \version $Revision: 1.1 $
00006 *  \date    $Date: 2003/07/04 16:19:43 $
00007 ************************************************************************************************/
00008 /*! 
00009 *
00010 ************************************************************************************************/
00011 
00012 #include "KalmanFilter.h"
00013 #include "KalmanFilterHistory.h"
00014 #include <iostream>
00015 #include <fstream>
00016 using namespace std;
00017 
00018 Matrix SMD_Fmatrix(KalmanFilter*);
00019 Matrix SMD_Hmatrix(KalmanFilter*);
00020 Matrix SMD_Qmatrix(KalmanFilter*);
00021 Matrix SMD_Rmatrix(KalmanFilter*);
00022 
00023 int main() {
00024         
00025         /*! Declare and initialize intermediate variables */
00026         Vector x(2);
00027         Vector z(2);
00028         Vector zeroVector(2);
00029         double tLast = 0.0, t = 0.0;
00030         
00031         /*! Set intial conditions */
00032         x(1) = 1.0;
00033         
00034         /*! Prepare input file */
00035         ifstream iFile;
00036         ofstream oFile;
00037         
00038         /*! Open input and output files */
00039         iFile.open("smdmeasurements.txt");
00040         oFile.open("smdresults.txt");
00041         
00042         /*! Read in initial measurements */
00043         double z1, z2;
00044         iFile >> t >> z1 >> z2;
00045         z(1) = z1; z(2) = z2;
00046         
00047         /*! Set up intial kalman filter data */
00048         Matrix P(2,2);
00049         P(1,1) = 0.001; P(1,2) = 0.0;
00050         P(2,1) = 0.0;  P(2,2) = 0.001;
00051         Matrix K(2,2);
00052         K(1,1) = 1.0; K(1,2) = 0.0;
00053         K(2,1) = 0.0; K(2,2) = 1.0;
00054 
00055         /*! Declare and initalize filter and its history */
00056         KalmanFilter kf(P,K);
00057         KalmanFilterHistory myHist(&kf);
00058         
00059         
00060         /*! Set up intial sequential filter data */
00061         Vector states = x;
00062         Vector controls = zeroVector;
00063         Vector parameters = zeroVector;
00064         Vector measurements = z;
00065         
00066         kf.SetTimeOfEstimate(tLast);
00067         kf.SetTimeOfMeasurements(t);
00068         kf.SetStateVector(states);
00069         kf.SetControlVector(controls);
00070         kf.SetMeasurementVector(measurements);
00071         kf.SetParameterVector(parameters);
00072 
00073         kf.SetSystemProcessNoiseMatrix(&SMD_Qmatrix);
00074         kf.SetMeasurementCovarianceMatrix(&SMD_Rmatrix);
00075         kf.SetStateJacobianMatrix(&SMD_Fmatrix);
00076         kf.SetMeasurementJacobianMatrix(&SMD_Hmatrix);
00077         
00078         /*! Output results to output file */
00079         oFile << kf.GetStateVector() << endl;
00080         
00081         while (iFile) {
00082                 
00083                 /*! Get next measurement */
00084                 iFile >> t >> z1 >> z2;
00085                 z(1) = z1; z(2) = z2;
00086                 
00087                 /*! Set new SF data including new measurements */
00088                 measurements = z;
00089                 kf.SetTimeOfMeasurements(t);
00090                 kf.SetMeasurementVector(measurements);
00091                         
00092                 /*! Estimate state using measurements */
00093                 kf.EstimateState();
00094                 
00095                 /*! Note time of last update */
00096                 tLast = t;
00097                 
00098                 /*! Output results to output file */
00099                 oFile << kf.GetStateVector() << endl;
00100                 
00101                 // write to the history
00102                 myHist.AppendHistory(tLast);
00103                 
00104         }
00105                 
00106         Matrix stateData;
00107         Matrix kalmanData;
00108         
00109         myHist.GetKalmanHistory(stateData, kalmanData);
00110         
00111         cout << "Here comes the history:" << endl << stateData;
00112         char pause;
00113         cout << "Press the any key." << endl;
00114         cin >> pause;
00115         
00116         cout << "Here's the rest:" << endl << kalmanData;
00117         
00118         iFile.close();
00119         oFile.close();
00120         
00121         return 1;
00122         
00123 }
00124 
00125 Matrix SMD_Fmatrix(KalmanFilter* kf) {
00126         
00127         Matrix temp(2,2);
00128         double k = 0.2, b = 0.1, m = 5.0;
00129         
00130         temp(1,1) = 0.0;    temp(1,2) = 1.0;
00131         temp(2,1) = -k / m; temp(2,2) = -b / m;
00132         
00133         return temp;
00134         
00135 };
00136 
00137 Matrix SMD_Hmatrix(KalmanFilter* kf) {
00138         
00139         Matrix temp(2,2);
00140         
00141         temp(1,1) = 1.0; temp(1,2) = 0.0;
00142         temp(2,1) = 0.0; temp(2,2) = 1.0;
00143         
00144         return temp;
00145         
00146 };
00147 
00148 Matrix SMD_Qmatrix(KalmanFilter* kf) {
00149         
00150         Matrix temp(2,2);
00151         
00152         temp(1,1) = 0.001; temp(1,2) = 0.0;
00153         temp(2,1) = 0.0; temp(2,2) = 0.001;
00154 
00155         return temp;
00156 
00157 };
00158 
00159 Matrix SMD_Rmatrix(KalmanFilter* kf) {
00160         
00161         Matrix temp(2,2);
00162         double x1_err = 0.2, x2_err = 0.02, x1_std, x2_std;
00163         
00164         x1_std = x1_err / pow(2.0, 0.5);
00165         x2_std = x2_err / pow(2.0, 0.5);
00166         
00167         temp(1,1) = pow(x1_std, 2.0); temp(1,2) = 0.0;
00168         temp(2,1) = 0.0;              temp(2,2) = pow(x1_std, 2.0);
00169         
00170         return temp;
00171         
00172 };
00173 
00174 // Do not change the comments below - they will be added automatically by CVS
00175 /*****************************************************************************
00176 * $Log: SMDexample.cpp,v $
00177 * Revision 1.1  2003/07/04 16:19:43  simpliciter
00178 * Moved.
00179 *
00180 * Revision 1.5  2003/06/28 00:44:58  simpliciter
00181 * Updated for completed KalmanHistory.
00182 *
00183 * Revision 1.4  2003/06/27 01:32:21  simpliciter
00184 * Added in history calls.
00185 *
00186 * Revision 1.3  2003/06/24 23:05:57  mavandyk
00187 * Corrected time handling to reflect changes in the filter classes.
00188 *
00189 * Revision 1.2  2003/06/24 16:02:00  simpliciter
00190 * Modified to not call structs.
00191 *
00192 * Revision 1.1  2003/06/23 21:57:57  mavandyk
00193 * A spring-mass-damper example estimation problem.
00194 *
00195 * Revision 1.4  2003/06/20 22:01:39  mavandyk
00196 * 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.
00197 *
00198 * Revision 1.3  2003/06/18 17:42:20  mavandyk
00199 * Changed the appropriate function input types to reflect changes in KalmanFilter.h.
00200 *
00201 * Revision 1.2  2003/06/12 22:59:26  mavandyk
00202 * Added header and footer comments.
00203 *
00204 * Revision 1.1  2003/06/12 16:29:41  mavandyk
00205 * This file replaces IterativeFilter.h, and only differs by replacing iterative with sequential.
00206 *
00207 * Revision 1.1  2003/06/11 15:45:08  simpliciter
00208 * Initial submission, pulled out of KalmanFilter.h.
00209 * May require (desire) additional pure virtual functions.
00210 *
00211 *
00212 ******************************************************************************/

Generated on Wed Sep 5 12:54:25 2007 for DSACSS Operational Code by  doxygen 1.3.9.1