QuaternionEKFObserver.h

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file QuaternionEKFObserver.h
 *  \brief  Estimation of the Quaternion and angular rates. Based on the Multiplicatimve Quaternion
 *              Formulation in "Optimal Estimation of Dynamic Systems" by Crassidis and Junkins.
 *  \author     $Author: jayhawk_hokie $
 *  \version $Revision: 1.8 $
 *  \date    $Date: 2007/07/24 08:42:15 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*
*/
//////////////////////////////////////////////////////////////////////////////////////////////////

#ifndef __SSSL_QUATERNION_EKF_OBSERVER_H__
#define __SSSL_QUATERNION_EKF_OBSERVER_H__


#include <iostream>
#include <cmath>
#include <fstream>

#include "dsacssinterface.h"

#include "Base/Whorl.h"
#include "rotation/Rotation.h"
#include "matrix/Matrix.h"
#include "utils/Time.h"

#include "Utils/matrixFunctionInterface.h"

#include "Sensors/DMUAccelerometer.h"
#include "Sensors/DMURateGyro.h"
#include "Sensors/Magnetometer.h"

#include "Observer.h"
#include "triadObserver.h"


extern "C" 
{
        #include "Hardware/satctl.h"
        #include "Hardware/common.h"
}


using namespace std;
using namespace O_SESSAME;

class QuaternionEKFObserver : public Observer 
{
public:

        /////////////////////////////////////////////////////
        // Contructors/Deconstructors
        ////////////////////////////////////////////////////
        
        /*! \brief Default Constructor */
        QuaternionEKFObserver( );
       
        /*! \brief Create a Quaternion Extended Kalman Filter Observer from XML handle and whorl object
         *  @param handle XML handle with current whorl as starting node
         *  @param ptr_whorl Pointer to a whorl object
         */
        QuaternionEKFObserver( TiXmlHandle handle, Whorl *ptr_whorl ); 

        /*! \brief Default Deconstructor */
        ~QuaternionEKFObserver( ); 

        /////////////////////////////////////////////////////
        // Facilitators
        /////////////////////////////////////////////////////

        /*! \brief Initialize the observer */
        int Initialize( );

        /*! \brief Deinitialize the observer. */
        int Deinitialize( );

        /*! Run is the funtion that implements the estimator in the derived class Filter. */
        int Run( );

        /*! \brief Start the Quaternion Extended Kalman Filter routine in a separate thread. */
        void StartEKF( );

        /*! \brief Friend function split in a new thread that runs the Quaternion Extended Kalman Filter routine.  
         *  @param arg the argument is set to be a "this" pointer cast as a void pointer
         */
        friend void* QuaternionEKF( void* arg );

        friend void* QuaternionEKFOriginal( void* arg );

        /*! \brief Stop the Quaternion Extended Kalman Filter routine.
         *  This function does not need to be called a t the end of the program.
         *  The thread will be closed automatically when the program ends.
         */
        void StopEKF( );

        void InitializeFilter( );

        void MurrellAlgorithm( );

        Vector SingleMeasurement( Vector _inertial, Vector _measurement, Vector _dXhatMinus, Matrix _estimatedAttitude, Matrix _sensorNoise );
        
        void MeasurementUpdate( );

        void StateCovarianceUpdate( );

        void StateCovarianceUpdate( Vector dXhatPlus );

        void StateTransitionMatrix( );
        
        void Parse(TiXmlHandle handle);
        
        /////////////////////////////////////////////////
        // Mutators
        ////////////////////////////////////////////////


        /////////////////////////////////////////////////
        // Inspectors
        /////////////////////////////////////////////////

        /*! \brief Get Current Measurements from Accelerometers, Angular Rates Gyros, and Magnetometer.
         *  The values are stored in member variables. */
        void GetCurrentMeasurements( );

        void GetInitialGuessTriad( );

        void CheckMagnetometerMeasurement( );

protected:

private:

        /*! \brief Member variable of Whorl type */
        Whorl* m_whorl;

        // inertial vectors
        Vector m_magnetometerInertial; // [magnetometer counts]
        Vector m_accelerometerInertial; // [m/s^2]
        
        // Body Vectors
        Vector m_measurementsBody; // magnetometer and accelerometer
//      Vector *m_magnetometerBody; // [magnetometer counts]
//              Vector *m_accelerometerBody; // [m/s^2]
        Vector m_angularRates; // [rad/s]

        Vector m_rsensor; 

        ssfTime m_currentMeasurementTime;
        ssfTime m_previousMeasurementTime;

        /*! \brief Time step inside EKF */
        ssfTime m_stepTime;
        /*! \brief Duration Time inside EKF */
        ssfTime m_durationTime;

        Vector m_BM;

        Matrix m_R;
        double m_sigmav;
        double m_sigmau;
        double m_P0a;
        double m_P0b;

        /*! \brief state vector comprised of quaternion and angular rates */    
        Vector m_outputState;

        /*! \brief Kalman Gain */
        Matrix m_K;

        /*! \brief */
        Vector m_qminus;
        
        /*! \brief attitude estimate */
        Vector m_qplus;

        Matrix m_H;

        Vector m_h;

        Vector m_angularRatehat;

        Vector m_previousMeasurement;

        /*! \brief State Estimate Error Covariance Matrix */
        Matrix m_Pminus;
        
        /*! \brief Measurement Update Estimate Covariance Matrix */
        Matrix m_Pplus;

        Matrix m_phi;

        int m_magnetometerCondition;

        int m_initialRange;
        int m_finalRange;

        /////////////////////////////
        // Threads
        ////////////////////////////

        /*! \brief Member pthread for splitting off EKF routine */
        pthread_t m_EKFThreadID;

        /*! \brief Memeber mutex lock variable*/
        pthread_mutex_t m_Mutex;
        
        int m_exitCondition;
};



#endif

// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: QuaternionEKFObserver.h,v $
*       Revision 1.8  2007/07/24 08:42:15  jayhawk_hokie
*       Removed matrix pointers.
*
*       Revision 1.7  2007/06/19 20:00:07  jayhawk_hokie
*       Changed inverse matrix function to basic algebra expression for a 3x3 matrix. Should reduce work load on PC104.
*
*       Revision 1.6  2007/06/18 13:31:34  jayhawk_hokie
*       Added Murrell's Algorithm to EKF Observer.
*
*       Revision 1.5  2007/05/30 23:46:39  jayhawk_hokie
*       Modified thread termination.
*
*       Revision 1.4  2007/05/29 21:39:00  jayhawk_hokie
*       Fixed deconstructor seg fault.
*
*       Revision 1.3  2007/05/29 15:30:01  jayhawk_hokie
*       Modified Deconstructor
*
*       Revision 1.2  2007/05/23 22:07:03  jayhawk_hokie
*       Implemented ignoring magnetometer measurement due to magnitude spikes.
*
*       Revision 1.1  2007/05/21 17:47:48  jayhawk_hokie
*       Initial Submission
*
*
*
*
******************************************************************************/

---