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

Eulerexample.cpp

Go to the documentation of this file.
00001 /************************************************************************************************/
00002 /*! \file Eulerexample.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 "ExtendedKalmanFilter.h"
00013 #include <iostream>
00014 #include <fstream>
00015 using namespace std;
00016 
00017 Matrix Euler_Fmatrix(KalmanFilter*);
00018 Matrix Euler_Hmatrix(KalmanFilter*);
00019 Matrix Euler_Qmatrix(KalmanFilter*);
00020 Matrix Euler_Rmatrix(KalmanFilter*);
00021 
00022 Vector Euler_stateRHSFunc(double t, Vector initialState, Vector controls, Vector params);
00023 Vector Euler_measurementRHSFunc(ExtendedKalmanFilter* ekf);
00024 
00025 int main() {
00026         
00027         /*! Declare and initialize intermediate variables */
00028         Vector x(3);
00029         Vector z(3);
00030         Vector zeroVector(3);
00031         double tLast = 0.0, t = 0.0;
00032         
00033         /*! Set intial conditions */
00034         x(1) = 0.1745;
00035         x(2) = 0.1745;
00036         x(3) = 0.1745;
00037         Vector p(3);
00038         p(1) = 2.0;
00039         p(2) = 3.0;
00040         p(3) = 4.0;
00041         
00042         /*! Prepare input file */
00043         ifstream iFile;
00044         ofstream oFile;
00045         
00046         /*! Open input and output files */
00047         iFile.open("eulermeasurements.txt");
00048         oFile.open("eulerresults.txt");
00049         
00050         /*! Read in initial measurements */
00051         double z1, z2, z3;
00052         iFile >> t >> z1 >> z2 >> z3;
00053         z(1) = z1; z(2) = z2; z(3) = z3;
00054         
00055         /*! Set up intial kalman filter data */
00056         Matrix P(3,3);
00057         P(1,1) = 0.001; P(1,2) = 0.0;   P(1,3) = 0.0;
00058         P(2,1) = 0.0;   P(2,2) = 0.001; P(2,3) = 0.0;
00059         P(3,1) = 0.0;   P(3,2) = 0.0;   P(3,3) = 0.001;
00060         Matrix K(3,3);
00061         K(1,1) = 0.001; K(1,2) = 0.0;   K(1,3) = 0.0;
00062         K(2,1) = 0.0;   K(2,2) = 0.001; K(2,3) = 0.0;
00063         K(3,1) = 0.0;   K(3,2) = 0.0;   K(3,3) = 0.001;
00064 
00065         /*! Declare and initalize filter */
00066         ExtendedKalmanFilter ekf;
00067         
00068         ekf.SetCovarianceMatrix(P);
00069         ekf.SetKalmanGainMatrix(K);
00070         
00071         /*! Set up intial sequential filter data */
00072         Vector states = x;
00073         Vector controls = zeroVector;
00074         Vector parameters = p;
00075         Vector measurements = z;
00076         
00077         ekf.SetTimeOfEstimate(tLast);
00078         ekf.SetTimeOfMeasurements(t);
00079         ekf.SetStateVector(states);
00080         ekf.SetControlVector(controls);
00081         ekf.SetMeasurementVector(measurements);
00082         ekf.SetParameterVector(parameters);
00083 
00084         ekf.SetSystemProcessNoiseMatrix(&Euler_Qmatrix);
00085         ekf.SetMeasurementCovarianceMatrix(&Euler_Rmatrix);
00086         ekf.SetStateJacobianMatrix(&Euler_Fmatrix);
00087         ekf.SetMeasurementJacobianMatrix(&Euler_Hmatrix);
00088         
00089         ekf.SetStateRHS(&Euler_stateRHSFunc);
00090         ekf.SetMeasurementRHS(&Euler_measurementRHSFunc);
00091         
00092         /*! Output results to output file */
00093         oFile << t << " " << ~ekf.GetStateVector();
00094         
00095         while (iFile) {
00096                 
00097                 /*! Get next measurement */
00098                 iFile >> t >> z1 >> z2 >> z3;
00099                 z(1) = z1; z(2) = z2; z(3) = z3;
00100                 
00101                 /*! Set new SF data including new measurements */
00102                 measurements = z;
00103                 ekf.SetTimeOfMeasurements(t);
00104                 ekf.SetMeasurementVector(measurements);
00105                 
00106                 /*! Estimate state using measurements */
00107                 ekf.EstimateState();
00108                 
00109                 /*! Note time of last update */
00110                 tLast = t;
00111                 
00112                 /*! Output results to output file */
00113                 oFile << t << " " << ~ekf.GetStateVector();
00114                 
00115         }
00116         
00117         cout << "I made it all the way through!" << endl;
00118         
00119         iFile.close();
00120         oFile.close();
00121         
00122         return 1;
00123         
00124 }
00125 
00126 Matrix Euler_Fmatrix(KalmanFilter* ekf) {
00127         
00128         Matrix temp(3,3);
00129         
00130         Vector state  = ekf->GetStateVector();
00131         Vector params = ekf->GetParameterVector();
00132         
00133         double w1 = state(1),  w2 = state(2),  w3 = state(3),
00134                I1 = params(1), I2 = params(2), I3 = params(3);
00135         
00136         temp(1,1) = 0.0;             temp(1,2) = (I2-I3)/I1 * w3; temp(1,3) = (I2-I3)/I1 * w2;
00137         temp(2,1) = (I3-I1)/I2 * w3; temp(2,2) = 0.0;             temp(2,3) = (I3-I1)/I2 * w1;
00138         temp(3,1) = (I1-I2)/I3 * w1; temp(3,2) = (I1-I2)/I3 * w1; temp(3,3) = 0.0;
00139         
00140         return temp;
00141         
00142 };
00143 
00144 Matrix Euler_Hmatrix(KalmanFilter* kf) {
00145         
00146         Matrix temp(3,3);
00147         
00148         temp(1,1) = 1.0; temp(1,2) = 0.0; temp(1,3) = 0.0;
00149         temp(2,1) = 0.0; temp(2,2) = 1.0; temp(2,3) = 0.0;
00150         temp(3,1) = 0.0; temp(3,2) = 0.0; temp(3,3) = 1.0;
00151         
00152         return temp;
00153         
00154 };
00155 
00156 Matrix Euler_Qmatrix(KalmanFilter* kf) {
00157         
00158         Matrix temp(3,3);
00159         
00160         temp(1,1) = 0.001; temp(1,2) = 0.0;   temp(1,3) = 0.0;
00161         temp(2,1) = 0.0;   temp(2,2) = 0.001; temp(2,3) = 0.0;
00162         temp(3,1) = 0.0;   temp(3,2) = 0.0;   temp(3,3) = 0.001;
00163 
00164         return temp;
00165 
00166 };
00167 
00168 Matrix Euler_Rmatrix(KalmanFilter* ekf) {
00169         
00170         Matrix temp(3,3);
00171         double gyro_err = 0.005, gyro_std = gyro_err / pow(2.0, 0.5);
00172         
00173         temp(1,1) = pow(gyro_std, 2.0); temp(1,2) = 0.0;                temp(1,3) = 0.0;
00174         temp(2,1) = 0.0;                temp(2,2) = pow(gyro_std, 2.0); temp(2,3) = 0.0;
00175         temp(3,1) = 0.0;                temp(3,2) = 0.0;                temp(3,3) = pow(gyro_std, 2.0);
00176         
00177         return temp;
00178         
00179 };
00180 
00181 Vector Euler_stateRHSFunc(double t, Vector initialState, Vector controls, Vector params) {
00182 
00183         Vector w = initialState;
00184 
00185         Matrix MOI(3,3);
00186 
00187         MOI(1,1) = params(1); MOI(1,2) = 0.0;       MOI(1,3) = 0.0;
00188         MOI(2,1) = 0.0;       MOI(2,2) = params(2); MOI(2,3) = 0.0;
00189         MOI(3,1) = 0.0;       MOI(3,2) = 0.0;       MOI(3,3) = params(3);
00190 
00191         Matrix MOIinv(3,3);
00192 
00193         MOIinv(1,1) = 1.0/params(1); MOIinv(1,2) = 0.0;           MOIinv(1,3) = 0.0;
00194         MOIinv(2,1) = 0.0;           MOIinv(2,2) = 1.0/params(2); MOIinv(2,3) = 0.0;
00195         MOIinv(3,1) = 0.0;           MOIinv(3,2) = 0.0;           MOIinv(3,3) = 1.0/params(3);
00196         
00197         Matrix wsk(3,3);
00198 
00199         wsk(1,1) =  0.0;  wsk(1,2) = -w(3); wsk(1,3) =  w(2);
00200         wsk(2,1) =  w(3); wsk(2,2) =  0.0;  wsk(2,3) = -w(1);
00201         wsk(3,1) = -w(2); wsk(3,2) =  w(1); wsk(3,3) =  0.0;
00202         
00203         return (MOIinv * (-wsk * MOI * w));
00204         
00205 };
00206 
00207 Vector Euler_measurementRHSFunc(ExtendedKalmanFilter* ekf) { return ekf->GetStateVector(); };
00208 
00209 // Do not change the comments below - they will be added automatically by CVS
00210 /*****************************************************************************
00211 * $Log: Eulerexample.cpp,v $
00212 * Revision 1.1  2003/07/04 16:19:43  simpliciter
00213 * Moved.
00214 *
00215 * Revision 1.2  2003/06/30 21:07:03  mavandyk
00216 * Altered output lines for more clarity and easier retrival of the data.
00217 *
00218 * Revision 1.1  2003/06/27 22:44:23  mavandyk
00219 * An example for the EKF class.
00220 *
00221 * Revision 1.3  2003/06/24 23:05:57  mavandyk
00222 * Corrected time handling to reflect changes in the filter classes.
00223 *
00224 * Revision 1.2  2003/06/24 16:02:00  simpliciter
00225 * Modified to not call structs.
00226 *
00227 * Revision 1.1  2003/06/23 21:57:57  mavandyk
00228 * A spring-mass-damper example estimation problem.
00229 *
00230 * Revision 1.4  2003/06/20 22:01:39  mavandyk
00231 * 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.
00232 *
00233 * Revision 1.3  2003/06/18 17:42:20  mavandyk
00234 * Changed the appropriate function input types to reflect changes in KalmanFilter.h.
00235 *
00236 * Revision 1.2  2003/06/12 22:59:26  mavandyk
00237 * Added header and footer comments.
00238 *
00239 * Revision 1.1  2003/06/12 16:29:41  mavandyk
00240 * This file replaces IterativeFilter.h, and only differs by replacing iterative with sequential.
00241 *
00242 * Revision 1.1  2003/06/11 15:45:08  simpliciter
00243 * Initial submission, pulled out of KalmanFilter.h.
00244 * May require (desire) additional pure virtual functions.
00245 *
00246 *
00247 ******************************************************************************/

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