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

EKFomega.cpp

Go to the documentation of this file.
00001 /************************************************************************************************/
00002 /*! \file EKFomega.cpp
00003 *  \brief An EKF to estimate the angular velocities in the simple form of Euler's Eqns.
00004 *  \author $Author: rsharo $
00005 *  \version $Revision: 1.3 $
00006 *  \date    $Date: 2003/11/01 21:00:17 $
00007 ************************************************************************************************/
00008 /*! 
00009 *
00010 ************************************************************************************************/
00011 
00012 #include "ExtendedKalmanFilter.h"
00013 #include "KalmanFilterHistory.h"
00014 #include "utils/Time.h"
00015 #include <iostream>
00016 #include <fstream>
00017 using namespace std;
00018 using namespace O_SESSAME;
00019 
00020 Matrix state_Euler_Fmatrix(KalmanFilter*);
00021 Matrix state_Euler_Hmatrix(KalmanFilter*);
00022 Matrix state_Euler_Qmatrix(KalmanFilter*);
00023 Matrix state_Euler_Rmatrix(KalmanFilter*);
00024 
00025 Vector state_Euler_stateRHSFunc(double t, Vector initialState, Vector controls, Vector params);
00026 Vector state_Euler_measurementRHSFunc(ExtendedKalmanFilter* ekf);
00027 
00028 int main() {
00029         
00030         tick();         // start the timer.
00031                 
00032         /*! Declare and initialize intermediate variables */
00033         Vector states(3);
00034         Vector parameters(6);
00035         Vector measurements(3);
00036         Vector controls(3);                     // uninitialized:  remains a zero Vector for the open-loop case.
00037         double t = 0.0;
00038         
00039         /*! Set intial conditions */
00040         states(1) = 0.25;
00041         states(2) = 0.3;
00042         states(3) = 0.2;
00043         
00044         parameters(1) = 5.7;
00045         parameters(2) = 0.4;
00046         parameters(3) = 0.3;
00047         parameters(4) = 5.3;
00048         parameters(5) = 0.45;
00049         parameters(6) = 9.7;
00050                 
00051         
00052         /*! Prepare data files */
00053         ifstream iFile;
00054         ofstream oFile;
00055         
00056         /*! Open input and output files */
00057         iFile.open("DataFiles/eulermeasurements.txt");
00058         oFile.open("DataFiles/EKFomega_bestguess.txt");
00059         
00060         
00061         /*! Read in initial measurements */
00062         iFile >> t >> measurements(1) >> measurements(2) >> measurements(3);
00063 
00064 
00065 
00066         /*! Set up intial kalman filter data */
00067         Matrix covariance = eye(3);
00068         covariance *= 0.5;
00069         
00070         Matrix kalmanGain(3,3); // does not need to be initialized.
00071         
00072         
00073         /*! Declare and initalize state filter and history */
00074         ExtendedKalmanFilter state_ekf;
00075         KalmanFilterHistory state_ekfhist(&state_ekf);
00076         
00077         state_ekf.SetCovarianceMatrix(covariance);
00078         state_ekf.SetKalmanGainMatrix(kalmanGain);
00079         
00080         state_ekf.SetTimeOfEstimate(t);
00081         state_ekf.SetTimeOfMeasurements(t);
00082         state_ekf.SetStateVector(states);
00083         state_ekf.SetControlVector(controls);
00084         state_ekf.SetMeasurementVector(measurements);
00085         state_ekf.SetParameterVector(parameters);
00086 
00087         state_ekf.SetSystemProcessNoiseMatrix(&state_Euler_Qmatrix);
00088         state_ekf.SetMeasurementCovarianceMatrix(&state_Euler_Rmatrix);
00089         state_ekf.SetStateJacobianMatrix(&state_Euler_Fmatrix);
00090         state_ekf.SetMeasurementJacobianMatrix(&state_Euler_Hmatrix);
00091         
00092         state_ekf.SetStateRHS(&state_Euler_stateRHSFunc);
00093         state_ekf.SetMeasurementRHS(&state_Euler_measurementRHSFunc);
00094         
00095         state_ekfhist.AppendHistory(t);
00096         
00097         
00098         /* Run the loop! */
00099                 
00100         while (iFile) {
00101                 
00102                 /*! Get next measurement */
00103                 iFile >> t >> measurements(1) >> measurements(2) >> measurements(3);
00104 
00105                 /*! Set new SF data including new measurements */
00106                 state_ekf.SetTimeOfMeasurements(t);
00107                 state_ekf.SetMeasurementVector(measurements);
00108                 
00109                 /*! Estimate state using measurements */
00110                 state_ekf.EstimateState();
00111                 
00112                 /*! Update the history */
00113                 state_ekfhist.AppendHistory(t);
00114                 
00115         }
00116         
00117         oFile << state_ekfhist.GetHistory();
00118         
00119         cout << "Run completed @ " << tock() << endl;
00120         
00121         iFile.close();
00122         oFile.close();
00123         
00124         return 1;
00125         
00126 }
00127 
00128 
00129 
00130 
00131 
00132 
00133 Vector state_Euler_stateRHSFunc(double t, Vector initialState, Vector controls, Vector params) {
00134         
00135         // build the moment of inertial matrix from the parameter vector
00136         Matrix MOI(3,3);
00137         MOI(1,1) = params(1);   MOI(1,2) = params(2);   MOI(1,3) = params(3);
00138         MOI(2,1) = params(2);   MOI(2,2) = params(4);   MOI(2,3) = params(5);
00139         MOI(3,1) = params(3);   MOI(3,2) = params(5);   MOI(3,3) = params(6);
00140 
00141         // build angular velocity stuff
00142         Vector omega(3);
00143         omega = initialState;
00144         
00145         // build updated state vector
00146         Vector updatedState(3);
00147         updatedState = MOI/( -skew(omega) * MOI * omega );
00148                 
00149         return (updatedState);
00150         
00151 };
00152 
00153 
00154 /*! Right now, this is providing angular velocity feedback
00155  * For the one-millionth time, is this what we get from the rate gyros? */
00156 Vector state_Euler_measurementRHSFunc(ExtendedKalmanFilter* ekf) { 
00157         
00158         Vector temp(3);
00159         temp = ekf->GetStateVector(); 
00160         
00161         return(temp);
00162         
00163 };
00164 
00165 
00166 
00167 Matrix state_Euler_Fmatrix(KalmanFilter* ekf) {
00168         
00169         Vector state  = ekf->GetStateVector();
00170         Vector params = ekf->GetParameterVector();
00171         
00172         // build the moment of inertial matrix from the parameter vector
00173         Matrix MOI(3,3);
00174         MOI(1,1) = params(1);   MOI(1,2) = params(2);   MOI(1,3) = params(3);
00175         MOI(2,1) = params(2);   MOI(2,2) = params(4);   MOI(2,3) = params(5);
00176         MOI(3,1) = params(3);   MOI(3,2) = params(5);   MOI(3,3) = params(6);
00177         
00178         // build angular velocity stuff
00179         Vector omega(3);
00180         omega = state;
00181         
00182         // build the state Jacobian matrix, df/dx
00183         Matrix temp(3,3);
00184         temp =  MOI/( -skew(omega) * MOI + skew(MOI*omega) );
00185         
00186         return temp;
00187         
00188 };
00189 
00190 Matrix state_Euler_Hmatrix(KalmanFilter* ekf) {
00191         
00192         Matrix temp(3,3);
00193         temp = eye(3);
00194         
00195         return temp;
00196         
00197 };
00198 
00199 Matrix state_Euler_Qmatrix(KalmanFilter* ekf) {
00200         
00201         Matrix temp = eye(3);
00202         temp *= 0.002;
00203 
00204         return temp;
00205 
00206 };
00207 
00208 Matrix state_Euler_Rmatrix(KalmanFilter* ekf) {
00209         
00210         Matrix temp = eye(3);
00211         double gyro_err = 1.0, gyro_std = gyro_err / pow(2.0, 0.5);
00212         
00213         temp *= pow(gyro_std, 2.0);
00214         
00215         return temp;
00216         
00217 };
00218 
00219 
00220 
00221 
00222 
00223 
00224 
00225 
00226 // Do not change the comments below - they will be added automatically by CVS
00227 /*****************************************************************************
00228 * $Log: EKFomega.cpp,v $
00229 * Revision 1.3  2003/11/01 21:00:17  rsharo
00230 * changed "Time.h" includes to "utils/Time.h" includes. Also eliminated excess compile warnings.
00231 *
00232 * Revision 1.2  2003/07/11 14:10:06  simpliciter
00233 * Good gains.
00234 *
00235 * Revision 1.1  2003/07/10 20:17:45  simpliciter
00236 * These work.
00237 *
00238 *
00239 ******************************************************************************/

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