DSACSS Operational Code: atterrbiasObserver.cpp Source File

00001 //////////////////////////////////////////////////////////////////////////////////////////////////
00002 /*! \file       atterrbiasObserver.h
00003  *  \brief      The atterrbiasObserver class inherits the functionality of the Observer class.  This should be the default observer for the MotionPak-only sensor configuration.
00004  *  \author     $Author: simpliciter $
00005  *  \version    $Revision: 1.1 $
00006  *  \date       $Date: 2004/07/29 18:22:57 $
00007 *//////////////////////////////////////////////////////////////////////////////////////////////////
00008 
00009 
00010 #include "atterrbiasObserver.h"
00011 #include <iostream>
00012 
00013 // set up the function pointers
00014 Vector ab_stateRHSFunc(double t, Vector initialState, Vector controls, Vector params);
00015 Matrix ab_Fmatrix(KalmanFilter*);
00016 Vector ab_measurementRHSFunc(ExtendedKalmanFilter* ekf);
00017 Matrix ab_Hmatrix(KalmanFilter*);
00018 Matrix ab_Qmatrix(KalmanFilter*);
00019 Matrix ab_Rmatrix(KalmanFilter*);
00020 Matrix otimes(Vector vec4);
00021 // done setting up function pointers
00022 
00023 
00024 int atterrbiasObserver::Initialize() {
00025 
00026         // we don't need the control vector for this filter, so set it to a constant zero value
00027         Vector zero3(3);
00028         m_control = zero3;
00029 
00030         // initial conditions and filter setup
00031         Matrix P(6,6);
00032         P = 0.1 * eye(6);
00033         ekf.SetCovarianceMatrix(P);
00034 
00035         Matrix K(6,6);
00036         ekf.SetKalmanGainMatrix(K);
00037         
00038         ekf.SetStateRHS(&ab_stateRHSFunc);
00039         ekf.SetStateJacobianMatrix(&ab_Fmatrix);
00040         
00041         ekf.SetMeasurementRHS(&ab_measurementRHSFunc);
00042         ekf.SetMeasurementJacobianMatrix(&ab_Hmatrix);
00043         
00044         ekf.SetSystemProcessNoiseMatrix(&ab_Qmatrix);
00045         ekf.SetMeasurementCovarianceMatrix(&ab_Rmatrix);
00046         
00047         return 0;
00048         
00049 };
00050 
00051 int atterrbiasObserver::Run() {
00052 
00053 
00054 ////////////////////////////////////////////////// APPLIED CONTROL
00055         ekf.SetControlVector(m_control);        // constant zero.  not used.
00056         
00057 
00058 ////////////////////////////////////////////////// MEASUREMENT
00059         // get measurements from the MotionPak and pull them into a Vector of doubles
00060         m_measurements[0] = m_whorl->GetRateGyro(string("dmu_rg1"))->GetMeasurement();
00061         m_measurements[1] = m_whorl->GetRateGyro(string("dmu_rg2"))->GetMeasurement();
00062         m_measurements[2] = m_whorl->GetRateGyro(string("dmu_rg3"))->GetMeasurement();
00063         m_measurements[3] = m_whorl->GetAccelerometer(string("dmu_acc1"))->GetMeasurement();
00064         m_measurements[4] = m_whorl->GetAccelerometer(string("dmu_acc2"))->GetMeasurement();
00065         m_measurements[5] = m_whorl->GetAccelerometer(string("dmu_acc3"))->GetMeasurement();
00066         
00067         Vector z(6);
00068         z(1) = m_measurements[0].GetAsDouble();
00069         z(2) = m_measurements[1].GetAsDouble();
00070         z(3) = m_measurements[2].GetAsDouble();
00071         z(4) = m_measurements[3].GetAsDouble();
00072         z(5) = m_measurements[4].GetAsDouble();
00073         z(6) = m_measurements[5].GetAsDouble();
00074 
00075         ekf.SetMeasurementVector(z);
00076         
00077 
00078 ////////////////////////////////////////////////// TIME
00079         // calculate the delta-time since the last estimate
00080         timeval measurementTime, currentTime;
00081         m_measurements[0].GetTime(measurementTime);
00082         currentTime = m_whorl->GetTimeOfEstimate();
00083         double deltat = static_cast<double>(measurementTime.tv_sec - currentTime.tv_sec) + (static_cast<double>(measurementTime.tv_usec - currentTime.tv_usec)) * 1e-6;
00084 
00085         double zero = 0.0;
00086         ekf.SetTimeOfEstimate(zero);                    
00087         ekf.SetTimeOfMeasurements(deltat);              
00088         
00089         
00090 ////////////////////////////////////////////////// REFERENCE STATE
00091         // get current state
00092         m_qomstate = m_whorl->GetState();
00093 
00094         // in this filter we don't need to know the mass properties of the system.
00095         // however, we do need the reference state.
00096         // convenient, eh?  param --> qom_{ref}
00097         // we do assume that bref = 0 for all time
00098         ekf.SetParameterVector(m_qomstate);
00099 
00100 
00101 ////////////////////////////////////////////////// FILTER STATE
00102         // rewind filter state back to zero
00103         m_abstate(1) = 0.0; m_abstate(2) = 0.0; m_abstate(3) = 0.0; 
00104         m_abstate(4) = 0.0; m_abstate(5) = 0.0; m_abstate(6) = 0.0;
00105         ekf.SetStateVector(m_abstate);                  // set the state vector
00106         
00107         
00108 ////////////////////////////////////////////////// RUN THE FILTER
00109         ekf.EstimateState();
00110         
00111         
00112 ////////////////////////////////////////////////// RESET STATE
00113         m_abstate = ekf.GetStateVector();
00114 
00115         // propagate the quaternion deltat seconds
00116         Vector omegabar(4);
00117         omegabar(_(1,3)) = m_qomstate(_(5,7));
00118 //      m_newq = expm( 0.5 * otimes(omegabar) * deltat ) * m_qomstate(_(1,4));  
00119 //      m_newq = m_newq ./ norm(m_newq);
00120 cout << "oops.  someone needs to write a matrix exponential function, or integrate numerically!" << endl;
00121 
00122         // update the quaternion based on the measurements
00123         Vector errorquat(4);
00124         double denom;
00125         denom = 1 + m_abstate(1)*m_abstate(1) + m_abstate(2)*m_abstate(2) + m_abstate(3)*m_abstate(3);
00126         denom = sqrt( denom );
00127 
00128         errorquat(4) = 1.0 / denom;
00129         errorquat(1) = m_abstate(_(1,3))(1) / denom;
00130         errorquat(2) = m_abstate(_(1,3))(2) / denom;
00131         errorquat(3) = m_abstate(_(1,3))(3) / denom;
00132         
00133         m_qomstate(_(1,4)) = otimes( errorquat ) * m_newq;
00134 
00135         m_qomstate(_(5,7)) = z(_(1,3)) - m_abstate(_(4,6));
00136 
00137 ////////////////////////////////////////////////// UPDATE SYSTEM VARIABLES
00138         m_whorl->SetState(m_qomstate);
00139         m_whorl->SetTimeOfEstimate(measurementTime);
00140         
00141         
00142         return 0;
00143         
00144 };
00145 
00146 
00147  
00148 
00149 Vector ab_stateRHSFunc(double t, Vector abstate, Vector zero, Vector qomref) {
00150         
00151         // split up the state
00152         Vector atterr(3);
00153         atterr = abstate(_(1,3));
00154         
00155         Vector rgbias(3);
00156         rgbias(_(1,3)) = abstate(_(4,6));
00157 
00158 
00159         // split up the reference state
00160         Vector qref(4);
00161         qref = qomref(_(1,4));
00162 
00163         Vector omref(3); 
00164         omref(_(1,3)) = qomref(_(5,7));
00165 
00166         Vector bref(3); // = [0 0 0]';
00167 
00168 
00169         // build RHS equations
00170         Vector stateDOT(6);
00171         stateDOT(_(1,3)) = skew(omref)*atterr + ( eye(3) + 0.25*atterr*~atterr + 0.5*skew(atterr) )*( bref - rgbias );
00172         //stateDOT(_(4,6)) = rgbiasDot = zero(3)
00173                 
00174         return (stateDOT);
00175         
00176 };
00177 
00178 
00179 Matrix ab_Fmatrix(KalmanFilter* ekf) {
00180         
00181         Vector abstate  = ekf->GetStateVector();
00182         Vector qomref = ekf->GetParameterVector();
00183         
00184         // split up the state
00185         Vector atterr(3);
00186         atterr = abstate(_(1,3));
00187         
00188         Vector rgbias(3);
00189         rgbias(_(1,3)) = abstate(_(4,6));
00190 
00191 
00192         // split up the reference state
00193         Vector qref(4);
00194         qref = qomref(_(1,4));
00195 
00196         Vector omref(3); 
00197         omref(_(1,3)) = qomref(_(5,7));
00198         
00199         Vector bref(3); // = [0 0 0]';
00200 
00201 
00202         // build the state Jacobian matrix, df/dx
00203         Matrix FMat(6,6);
00204         
00205         FMat(_(1,3),_(1,3)) = -skew(omref) + 0.25*~atterr*( bref - rgbias )*eye(3) + 0.25*atterr*( bref - rgbias ) - 0.5*skew( bref - rgbias );
00206         FMat(_(1,3),_(4,6)) = -( eye(3) + 0.25*atterr*~atterr + 0.5*skew( atterr ) );
00207         //FMat(_(4,6),_(1,6)) = zeros;
00208                 
00209         return FMat;
00210         
00211 };
00212 
00213 
00214 Vector ab_measurementRHSFunc(ExtendedKalmanFilter* ekf) { 
00215         
00216         /**** HARD CODED IN TWO PLACES! ****/
00217         Vector sensorPosition(3);
00218         sensorPosition(1) = -4;
00219         sensorPosition(2) = -5;
00220         sensorPosition(3) = 2;
00221         sensorPosition *= 2.54/100.0;
00222 
00223         double gee = 9.81;
00224         Vector khat(3); khat(3) = 1;
00225         /**** HARD CODED IN TWO PLACES! ****/
00226         
00227         
00228         Vector qomref(7);
00229         qomref = ekf->GetParameterVector();
00230         
00231         Vector z(6);
00232         z = ekf->GetMeasurementVector();
00233         
00234         Vector abstate(6);
00235         abstate = ekf->GetStateVector();
00236         
00237         // split up the state
00238         Vector atterr(3);
00239         atterr = abstate(_(1,3));
00240         
00241         Vector rgbias(3);
00242         rgbias(_(1,3)) = abstate(_(4,6));
00243         
00244         
00245         // get the rotation matrix
00246         Vector errorquat(4);
00247         double denom;
00248         denom = 1 + abstate(1)*abstate(1) + abstate(2)*abstate(2) + abstate(3)*abstate(3);
00249         denom = sqrt( denom );
00250 
00251         errorquat(4) = 1.0 / denom;
00252         errorquat(1) = abstate(_(1,3))(1) / denom;
00253         errorquat(2) = abstate(_(1,3))(2) / denom;
00254         errorquat(3) = abstate(_(1,3))(3) / denom;
00255 
00256         Matrix Rbi(3,3);
00257         Rbi = otimes(errorquat) * qomref(_(1,4));
00258 
00259         Vector measFunc(3);
00260         measFunc(_(1,3)) = skew( z(_(1,3)) - rgbias ) * skew( z(_(1,3)) - rgbias ) * sensorPosition - gee*Rbi*khat;
00261         
00262         
00263         return(measFunc);
00264         
00265 };
00266 
00267 
00268 
00269 Matrix ab_Hmatrix(KalmanFilter* ekf) {
00270         
00271         /**** HARD CODED IN TWO PLACES! ****/
00272         Vector sensorPosition(3);
00273         sensorPosition(1) = -4;
00274         sensorPosition(2) = -5;
00275         sensorPosition(3) = 2;
00276         sensorPosition *= 2.54/100.0;
00277 
00278         double gee = 9.81;
00279         Vector khat(3); khat(3) = 1;
00280         /**** HARD CODED IN TWO PLACES! ****/
00281         
00282         
00283         Vector qomref(7);
00284         qomref = ekf->GetParameterVector();
00285         
00286         Vector z(6);
00287         z = ekf->GetMeasurementVector();
00288         
00289         Vector abstate(6);
00290         abstate = ekf->GetStateVector();
00291         
00292         // split up the state
00293         Vector atterr(3);
00294         atterr = abstate(_(1,3));
00295         
00296         Vector rgbias(3);
00297         rgbias(_(1,3)) = abstate(_(4,6));
00298         
00299         // get the rotation matrix
00300         Vector errorquat(4);
00301         double denom;
00302         denom = 1 + abstate(1)*abstate(1) + abstate(2)*abstate(2) + abstate(3)*abstate(3);
00303         denom = sqrt( denom );
00304 
00305         errorquat(4) = 1.0 / denom;
00306         errorquat(1) = abstate(_(1,3))(1) / denom;
00307         errorquat(2) = abstate(_(1,3))(2) / denom;
00308         errorquat(3) = abstate(_(1,3))(3) / denom;
00309 
00310         Matrix Rbi(3,3);
00311         Rbi = otimes(errorquat) * qomref(_(1,4));
00312 
00313         Vector r3(3);
00314         r3 = Rbi * khat;
00315 
00316         Matrix HMat(3,6);
00317         HMat(_(1,3),_(1,3)) = -gee * ( skew(r3) - r3*~atterr + 0.5*~atterr*r3*eye(1) + 0.5*atterr*~r3 );
00318         HMat(_(1,3),_(4,6)) = -skew( skew( z(_(1,3)) - rgbias ) * sensorPosition ) - skew( z(_(1,3)) - rgbias ) * skew( sensorPosition );
00319         
00320         return HMat;
00321         
00322 };
00323 
00324 Matrix ab_Qmatrix(KalmanFilter* ekf) {
00325         
00326         Matrix temp = eye(7);
00327         temp *= 0.1;
00328         
00329         return temp;
00330 
00331 };
00332 
00333 Matrix ab_Rmatrix(KalmanFilter* ekf) {
00334 
00335         Matrix temp(6,6);
00336 
00337         double gyro_err = 0.01, gyro_std = gyro_err / pow(2.0, 0.5);
00338         temp(_(1,3),_(1,3)) = eye(3) * pow(gyro_std, 2.0);
00339         
00340         double accel_err = 0.01, accel_std = accel_err / pow(2.0, 0.5);
00341         temp(_(4,6),_(4,6)) = eye(3) * pow(accel_std, 2.0);
00342         
00343         return temp;
00344         
00345 };
00346 
00347 
00348 
00349 Matrix otimes(Vector vec4) {
00350 
00351         Vector v123(3);
00352         v123(_(1,3)) = vec4(_(1,3));
00353 
00354         double v4;
00355         v4 = vec4(4);
00356 
00357         Matrix mm(4,4);
00358 
00359         mm(_(1,3),_(1,3)) = skew( v123 ) + v4*eye(3);
00360         mm(4,_(1,3)) = v123;
00361         mm(_(1,3),4) = ~v123;
00362         mm(4,4) = v4;
00363 
00364         return(mm);
00365 
00366 };
00367          
00368 
00369 
00370 
00371 
00372 
00373 
00374 
00375 
00376 // Do not change the comments below - they will be added automatically by CVS
00377 /*****************************************************************************
00378 *       $Log: atterrbiasObserver.cpp,v $
00379 *       Revision 1.1  2004/07/29 18:22:57  simpliciter
00380 *       The good news:  this file is well commented and should be easy to work with!
00381 *       The bad news:  it needs a matrix exponential function.  :-(  Sorry!
00382 *       Any questions, check out Jana's dissertation or send an email!
00383 *       
00384 *       
00385 *
00386 ******************************************************************************/
00387 
00388