DSACSS Operational Code: DefaultObserver.cpp Source File

00001 //////////////////////////////////////////////////////////////////////////////////////////////////
00002 /*! \file DefaultObserver.cpp
00003  *  \brief The default observer class inherits the functionality of the observer class and provides
00004  *         the space for the the actual filter code.
00005  *  \author     $Author: simpliciter $
00006  *  \version $Revision: 1.12 $
00007  *  \date    $Date: 2003/08/29 00:43:48 $
00008 *//////////////////////////////////////////////////////////////////////////////////////////////////
00009 /*  
00010 */
00011 //////////////////////////////////////////////////////////////////////////////////////////////////
00012 
00013 #include "DefaultObserver.h"
00014 
00015 Matrix Euler_Fmatrix(KalmanFilter* ekf);
00016 Matrix Euler_Hmatrix(KalmanFilter* kf);
00017 Matrix Euler_Qmatrix(KalmanFilter* kf);
00018 Matrix Euler_Rmatrix(KalmanFilter* kf);
00019 Vector Euler_stateRHSFunc(double t, Vector initialState, Vector controls, Vector params);
00020 Vector Euler_measurementRHSFunc(ExtendedKalmanFilter* ekf);
00021 
00022 /*! Initialize the filter */
00023 int DefaultObserver::Initialize() {
00024         
00025         /*! Set up intial kalman filter data */
00026         Matrix P(3,3);
00027         P(1,1) = 0.001; P(1,2) = 0.0;   P(1,3) = 0.0;
00028         P(2,1) = 0.0;   P(2,2) = 0.001; P(2,3) = 0.0;
00029         P(3,1) = 0.0;   P(3,2) = 0.0;   P(3,3) = 0.001;
00030         Matrix K(3,3);
00031         K(1,1) = 0.001; K(1,2) = 0.0;   K(1,3) = 0.0;
00032         K(2,1) = 0.0;   K(2,2) = 0.001; K(2,3) = 0.0;
00033         K(3,1) = 0.0;   K(3,2) = 0.0;   K(3,3) = 0.001;
00034         
00035         ekf.SetCovarianceMatrix(P);
00036         ekf.SetKalmanGainMatrix(K);
00037         
00038         ekf.SetSystemProcessNoiseMatrix(&Euler_Qmatrix);
00039         ekf.SetMeasurementCovarianceMatrix(&Euler_Rmatrix);
00040         ekf.SetStateJacobianMatrix(&Euler_Fmatrix);
00041         ekf.SetMeasurementJacobianMatrix(&Euler_Hmatrix);
00042         
00043         ekf.SetStateRHS(&Euler_stateRHSFunc);
00044         ekf.SetMeasurementRHS(&Euler_measurementRHSFunc);
00045         
00046         return 0;
00047         
00048 };
00049 
00050 /*! Run is the funtion that implements the estimator in the derived class Filter */
00051 int DefaultObserver::Run() {
00052         
00053         /** Get rate gyro measurements */
00054         m_measurements[0] = m_whorl->GetRateGyro(string("dmu_rg1"))->GetMeasurement();
00055         m_measurements[1] = m_whorl->GetRateGyro(string("dmu_rg2"))->GetMeasurement();
00056         m_measurements[2] = m_whorl->GetRateGyro(string("dmu_rg3"))->GetMeasurement();
00057         Vector z(3);
00058         z(1) = m_measurements[0].GetAsDouble();
00059         z(2) = m_measurements[1].GetAsDouble();
00060         z(3) = m_measurements[2].GetAsDouble();
00061         
00062         /** Perform time calculations */
00063         timeval measurementTime, currentTime;
00064         m_measurements[0].GetTime(measurementTime);
00065         currentTime = m_whorl->GetTimeOfEstimate();
00066         double relTimeOfMeasurement = static_cast<double>(measurementTime.tv_sec - currentTime.tv_sec) + (static_cast<double>(measurementTime.tv_usec - currentTime.tv_usec)) * 1e-6;
00067         
00068         /* Begin filter code --------------------------------- */
00069         
00070         /** Initialize filter */
00071         Vector state   = m_whorl->GetOmegaBL();
00072         Vector control = m_whorl->GetControl();
00073         Vector params  = m_whorl->GetParameter();
00074         double zero = 0.0;
00075         ekf.SetTimeOfEstimate(zero);             // set time of the last estimate
00076         ekf.SetTimeOfMeasurements(relTimeOfMeasurement); // set time of the measurements
00077         ekf.SetStateVector(state);                  // set the state vector
00078         ekf.SetControlVector(control);
00079         ekf.SetParameterVector(params);
00080         ekf.SetMeasurementVector(z);
00081         
00082         /*! Estimate state using measurements */
00083         ekf.EstimateState();
00084         
00085         /* End filter code --------------------------------- */
00086         // Update the global Whorl class
00087         state = ekf.GetStateVector();
00088         Vector fullState(7);
00089         fullState(4) = 1.0;
00090         fullState(_(5,7)) = state;
00091         m_whorl->SetState(fullState);
00092         m_whorl->SetTimeOfEstimate(measurementTime);
00093         
00094         return 0;
00095         
00096 };
00097 
00098 Matrix Euler_Fmatrix(KalmanFilter* ekf) {
00099         
00100         Vector state  = ekf->GetStateVector();
00101         Vector params = ekf->GetParameterVector();
00102         
00103         Matrix MOI(3,3);
00104 
00105         MOI(1,1) = params(1); MOI(1,2) = params(2); MOI(1,3) = params(3);
00106         MOI(2,1) = params(2); MOI(2,2) = params(4); MOI(2,3) = params(5);
00107         MOI(3,1) = params(3); MOI(3,2) = params(5); MOI(3,3) = params(6);
00108         
00109         Vector w = state;
00110         
00111         Matrix temp = MOI/( -skew(w)*MOI +  skew(MOI * w) );
00112         
00113         return temp;
00114         
00115 };
00116 
00117 Matrix Euler_Hmatrix(KalmanFilter* kf) {
00118         
00119         Matrix temp(3,3);
00120         
00121         temp(1,1) = 1.0; temp(1,2) = 0.0; temp(1,3) = 0.0;
00122         temp(2,1) = 0.0; temp(2,2) = 1.0; temp(2,3) = 0.0;
00123         temp(3,1) = 0.0; temp(3,2) = 0.0; temp(3,3) = 1.0;
00124         
00125         return temp;
00126         
00127 };
00128 
00129 Matrix Euler_Qmatrix(KalmanFilter* kf) {
00130         
00131         Matrix temp(3,3);
00132         
00133         temp(1,1) = 0.0001; temp(1,2) = 0.0;   temp(1,3) = 0.0;
00134         temp(2,1) = 0.0;   temp(2,2) = 0.001; temp(2,3) = 0.0;
00135         temp(3,1) = 0.0;   temp(3,2) = 0.0;   temp(3,3) = 0.001;
00136 
00137         return temp;
00138 
00139 };
00140 
00141 Matrix Euler_Rmatrix(KalmanFilter* ekf) {
00142         
00143         Matrix temp(3,3);
00144         double gyro_err = 0.005, gyro_std = gyro_err / pow(2.0, 0.5);
00145         
00146         temp(1,1) = pow(gyro_std, 2.0); temp(1,2) = 0.0;                temp(1,3) = 0.0;
00147         temp(2,1) = 0.0;                temp(2,2) = pow(gyro_std, 2.0); temp(2,3) = 0.0;
00148         temp(3,1) = 0.0;                temp(3,2) = 0.0;                temp(3,3) = pow(gyro_std, 2.0);
00149         
00150         return temp;
00151         
00152 };
00153 
00154 Vector Euler_stateRHSFunc(double t, Vector initialState, Vector controls, Vector params) {
00155 
00156         Vector w = initialState;
00157 
00158         Matrix MOI(3,3);
00159 
00160         MOI(1,1) = params(1); MOI(1,2) = params(2); MOI(1,3) = params(3);
00161         MOI(2,1) = params(2); MOI(2,2) = params(4); MOI(2,3) = params(5);
00162         MOI(3,1) = params(3); MOI(3,2) = params(5); MOI(3,3) = params(6);
00163         
00164 
00165         return (MOI / ( (-skew(w) * MOI * w) + controls) );
00166 };
00167 
00168 Vector Euler_measurementRHSFunc(ExtendedKalmanFilter* ekf) { return ekf->GetStateVector(); };
00169 
00170 // Do not change the comments below - they will be added automatically by CVS
00171 /*****************************************************************************
00172 *       $Log: DefaultObserver.cpp,v $
00173 *       Revision 1.12  2003/08/29 00:43:48  simpliciter
00174 *       Fixed bugs in F and RHS functions
00175 *       
00176 *       Revision 1.11  2003/08/18 19:09:30  mavandyk
00177 *       Fixed state update to include quaternion.
00178 *       
00179 *       Revision 1.10  2003/08/13 23:18:07  mavandyk
00180 *       Altered structure as outline in meeting.
00181 *       
00182 *       Revision 1.9  2003/08/06 21:49:11  mavandyk
00183 *       Added comments fixed class structure and runs with config parsig.
00184 *       
00185 *       Revision 1.8  2003/08/01 11:21:30  mavandyk
00186 *       Changed class structure.
00187 *       
00188 *       Revision 1.7  2003/07/22 21:22:52  mavandyk
00189 *       Fixed bugs.
00190 *       
00191 *       Revision 1.6  2003/07/21 21:53:48  mavandyk
00192 *       Fixed some syntax errors.
00193 *       
00194 *       Revision 1.5  2003/07/14 20:06:48  mavandyk
00195 *       Fixed calls to Whorl class, and handling of time and measurements.
00196 *       
00197 *       Revision 1.4  2003/07/10 19:51:02  mavandyk
00198 *       Fixed time handling.
00199 *       
00200 *       Revision 1.3  2003/07/07 20:47:44  mavandyk
00201 *       Changed the state RHS function to use the skew utility function.
00202 *       
00203 *       Revision 1.2  2003/07/07 20:43:34  mavandyk
00204 *       Fixed the Fmatrix function with JS's derivation of the Jacobian matrix for omegadot.
00205 *       
00206 *       Revision 1.1  2003/07/04 14:12:29  simpliciter
00207 *       Moved from src directory.
00208 *       
00209 *       Revision 1.1  2003/07/02 21:33:42  mavandyk
00210 *       The observer for the demo.
00211 *       
00212 *       Revision 1.1  2003/06/30 16:06:00  nicmcp
00213 *       replaced the controller files with the Controller files
00214 *       
00215 *       Revision 1.3  2003/06/30 15:51:57  nicmcp
00216 *       made some changes to the controller files
00217 *       
00218 *       Revision 1.2  2003/06/30 14:13:35  simpliciter
00219 *       Modified comments, added log blocks.
00220 *       
00221 *       
00222 *
00223 ******************************************************************************/