QuaternionEKFObserver.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file QuaternionEKFObserver.cpp
 *  \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.9 $
 *  \date    $Date: 2007/08/31 16:08:01 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*
*/
//////////////////////////////////////////////////////////////////////////////////////////////////

#include"QuaternionEKFObserver.h"


QuaternionEKFObserver::QuaternionEKFObserver( )
{ }

QuaternionEKFObserver::QuaternionEKFObserver( TiXmlHandle handle, Whorl* ptr_whorl )
{
        m_whorl = ptr_whorl;

        pthread_mutex_init( &m_Mutex, NULL );

        m_accelerometerInertial.initialize(3);
        m_magnetometerInertial.initialize(3);
        m_angularRates.initialize(3);
        m_measurementsBody.initialize(6);
        m_rsensor.initialize(3);
        m_R.initialize(6,6);
        
        m_outputState.initialize(7);
        m_previousMeasurement.initialize(6);
        //m_previousMeasurement.initialize(3);
        
        m_currentMeasurementTime=0;
        m_previousMeasurementTime=0;
        m_stepTime=0;
        m_durationTime=0;
        m_sigmav=0;
        m_sigmau=0;
        m_P0a=0;
        m_P0b=0;
        
        m_BM.initialize(3);
        
        m_K.initialize(6,6);
        m_qminus.initialize(4);
        m_qplus.initialize(4);
        m_H.initialize(6,6);
        m_h.initialize(6);
        m_angularRatehat.initialize(3);
        m_Pminus.initialize(6,6);
        m_Pplus.initialize(6,6);
        m_phi.initialize(6,6);

        m_initialRange = 1;
        m_finalRange = 6;
        
        m_magnetometerCondition = 0;
        
        m_exitCondition = 1;

        Parse( handle );
        Initialize( );

}

QuaternionEKFObserver::~QuaternionEKFObserver( )
{ 

        cerr << "Stop Quaternion EKF Routine" << endl;
        StopEKF( );

        //delete m_previousMeasurement;
        
        // Deallocating memory from mutex
        pthread_mutex_destroy( &m_Mutex );

}

int QuaternionEKFObserver::Initialize( )
{

        // Start EKF Routine
        cerr << "Start Quaternion EKF Routine" << endl;
        StartEKF( );
        usleep( 500000 );


        return( 0 );
}

/*! \brief Parse inertial measurement vectors from configuration file
        * @param handle XML handle with current whorl as starting node
        */
void QuaternionEKFObserver::Parse( TiXmlHandle handle )
{

        int response;

        // create handles with magnetometer and accelerometer nodes as starting points
        TiXmlHandle accelElement =  handle.FirstChild( m_whorl->GetSimulatorName( ) ).FirstChild("HARDWARE_PROPERTIES").FirstChild("DMU_ACCELEROMETER");
        TiXmlHandle magElement = handle.FirstChild( m_whorl->GetSimulatorName( ) ).FirstChild("HARDWARE_PROPERTIES").FirstChild("MAGNETOMETER");

        // get magnetometer inertial vector and normalize
        response = magElement.Child("INERTIAL_VECTOR", 0).Element() ->  QueryDoubleAttribute( "valueX", &(m_magnetometerInertial)(1) );
        checkResponse(response);
        response = magElement.Child("INERTIAL_VECTOR", 0).Element() ->  QueryDoubleAttribute( "valueY", &(m_magnetometerInertial)(2) );
        checkResponse(response);
        response = magElement.Child("INERTIAL_VECTOR", 0).Element() ->  QueryDoubleAttribute( "valueZ", &(m_magnetometerInertial)(3) );
        checkResponse(response);

        // Normalize magnetmeter inertial vector
        normalize( m_magnetometerInertial );

        // get accelerometer inertial vector and normalize
        response = accelElement.Child("INERTIAL_VECTOR", 0).Element() -> QueryDoubleAttribute( "valueX", &(m_accelerometerInertial)(1) );
        checkResponse(response);
        response = accelElement.Child("INERTIAL_VECTOR", 0).Element() -> QueryDoubleAttribute( "valueY", &(m_accelerometerInertial)(2) );
        checkResponse(response);
        response = accelElement.Child("INERTIAL_VECTOR", 0).Element() -> QueryDoubleAttribute( "valueZ", &(m_accelerometerInertial)(3) );
        checkResponse(response);

        // Normalize accelerometer inertial vector
        normalize( m_accelerometerInertial ); // [rad/s^2]

        // get motion pak sensor radius
        for (int i=0;i<3;i++)
        {
                response = handle.FirstChild( m_whorl->GetSimulatorName( ) ).FirstChild("PHYSICAL_PROPERTIES").FirstChild("SENSOR_RADIUS").Child("VECTOR",i)
                        .Element() -> QueryDoubleAttribute("value",&(m_rsensor)(i+1));
                checkResponse(response);
        }

        ////////////////
        ///////////////

        // This section sets the measurement covariance based on the variance for each sensor
        // Future move to xml
        (m_R)(1,1) = 2.324*1e7; // magnetometer x (counts)
        (m_R)(2,2) = 3.226*1e7; // magnetometer y (counts)
        (m_R)(3,3) = 2.394*1e7; // magnetometer z (counts)
        (m_R)(4,4) = 0.000106; // accelerometer x (m/s^2)
        (m_R)(5,5) = 0.000105; // accelerometer y (m/s^2)
        (m_R)(6,6) = 0.000102; // accelerometer z (m/s^2)

        // move to xml
        m_sigmav = sqrt( 0.00004 ); // rate gyro variance
        m_sigmau = 2e-5; // rate gyro bias variance

        // variables to use in covariance initialization
        // move to xml
        m_P0a= 8.46e-6;
        m_P0b = 9.4018e-13;
        
        // Bias initial vector
        // Future move to xml
        (m_BM)(1) = 0;
        (m_BM)(2) = 0;
        (m_BM)(3) = 0;
        
        /////////////////
        ////////////////
}

void QuaternionEKFObserver::StartEKF( )
{
        //pthread_create( &m_EKFThreadID, NULL, QuaternionEKF, (void*) this );
        pthread_create( &m_EKFThreadID, NULL, QuaternionEKFOriginal, (void*) this );
}

void* QuaternionEKF( void* arg )
{
        QuaternionEKFObserver *EKFObserver = (QuaternionEKFObserver*) arg;
        // Initialize Filter Parameters
        EKFObserver->InitializeFilter( );

        // Get Current Measurement
        EKFObserver->GetCurrentMeasurements( );
        
        EKFObserver->m_previousMeasurementTime = EKFObserver->m_currentMeasurementTime; 
        
        EKFObserver->m_stepTime = EKFObserver->m_currentMeasurementTime - EKFObserver->m_previousMeasurementTime; 

        EKFObserver->m_durationTime += EKFObserver->m_stepTime.GetSeconds( );

        // Get initial Guess from Triad Algorithm       
        EKFObserver->GetInitialGuessTriad( );
        
        // Murrell's Algorithm for computationally efficient attitude estimation
        EKFObserver->MurrellAlgorithm( );

        pthread_testcancel( );

        EKFObserver->m_previousMeasurement = EKFObserver->m_measurementsBody;

        int exit = 1;
        
        while( exit )
        {
                usleep( 100000 );
                
                pthread_testcancel( );

                // Get Current Measurement
                EKFObserver->GetCurrentMeasurements( );

                // Check Measurement
                EKFObserver->CheckMagnetometerMeasurement( );   
                
                EKFObserver->m_stepTime = EKFObserver->m_currentMeasurementTime - EKFObserver->m_previousMeasurementTime; 

                EKFObserver->m_durationTime += EKFObserver->m_stepTime.GetSeconds( );

                pthread_testcancel( );

                // Murrell's Algorithm for computationally efficient attitude estimation
                EKFObserver->MurrellAlgorithm( );

                pthread_testcancel( );
        
                //EKFObserver->MeasurementUpdate( );

                //pthread_testcancel( );

                EKFObserver->m_previousMeasurementTime = EKFObserver->m_currentMeasurementTime; // reset previous time measurement
                
                pthread_testcancel( );
                
                pthread_mutex_lock(&EKFObserver->m_Mutex); 
                        exit = EKFObserver->m_exitCondition;
                pthread_mutex_unlock(&EKFObserver->m_Mutex); 
        }

        return( arg );
}

void QuaternionEKFObserver::StopEKF( )
{
        // Lock output state vector
        pthread_mutex_lock(&m_Mutex);
                m_exitCondition = 0;
        // Unlock output state vector
        pthread_mutex_unlock(&m_Mutex);
        
        pthread_join( m_EKFThreadID, NULL );

        return;
}

void QuaternionEKFObserver::InitializeFilter( )
{

        // covariance initialization (diagonal)
        (m_Pminus)(1,1) = m_P0a;
                (m_Pminus)(2,2) = m_P0a;
                        (m_Pminus)(3,3) = m_P0a;
                                (m_Pminus)(4,4) = m_P0b;
                                        (m_Pminus)(5,5) = m_P0b;
                                                (m_Pminus)(6,6) = m_P0b;

        return;
}

void QuaternionEKFObserver::GetCurrentMeasurements( )
{
        // vector measurements for both time update and measurement update steps are taken here
        (m_measurementsBody)( _(1,3) ) = m_whorl->GetMagnetometer() -> GetVectorMeasurement( );
        
        string axis;
        for( int i =0; i < 3; i++ )
        {
                // set up a string to designate axis
                switch( i ) 
                {
                        case 0:
                                axis = "X";
                                break;
                        case 1:
                                axis = "Y";
                                break;
                        case 2:
                                axis = "Z";
                                break;
                        default:
                                break;
                }       

                /* Rate Gyros */
                (m_angularRates)(i+1) = m_whorl->GetRateGyro( "RATE_GYRO_" + axis ) -> GetMeasurement().GetAsDouble();
                /* Accelerometers */
                (m_measurementsBody)(i+4) = m_whorl->GetAccelerometer( "ACC_" + axis ) -> GetMeasurement().GetAsDouble();
        } 

        m_currentMeasurementTime = Now( );
        
        return;
}

void QuaternionEKFObserver::CheckMagnetometerMeasurement( )
{
        /* Check Magnetometer vector. If bad ignore measurement */
        double condition = norm2( (m_previousMeasurement)( _(1,3) ) ) - norm2( (m_measurementsBody)( _(1,3) ) );
        if ( fabs(condition) > 50 )
        {
                // ignore measurement
                m_magnetometerCondition = 1;
                m_initialRange = 4;
                m_finalRange = 6;

        }
        else
        { // update previous measurement
                m_magnetometerCondition = 0;
                m_previousMeasurement = m_measurementsBody; // good measurement
        }
        return;
}

void QuaternionEKFObserver::GetInitialGuessTriad( )
{
        // use Triad to get initial guess
        triadObserver initialGuess;
        initialGuess.SetMagnetometerBodyVector( (m_measurementsBody)( _(1,3)) );
        initialGuess.SetMagnetometerInertialVector( m_magnetometerInertial );
        initialGuess.SetAccelerometerBodyVector( (m_measurementsBody)( _(4,6)) );
        initialGuess.SetAccelerometerInertialVector( m_accelerometerInertial );
        
        DirectionCosineMatrix attitudeMatrix = initialGuess.GetAttitude( );
        m_qminus = attitudeMatrix.GetQuaternion( );

        return;
}       

void QuaternionEKFObserver::MurrellAlgorithm( )
{
        // set estimated attitude DCM (3x3)
        Quaternion qminus( m_qminus );
        Matrix estimatedAttitude(3,3);
        estimatedAttitude = DirectionCosineMatrix( qminus );

        // set dXhatMinus to zero (6x1)
        Vector dXhatMinus(6);

        // set dXhatPlus to zero (6x1)
        Vector dXhatPlus(6);

        // Magnetometer Measurement
        if( m_magnetometerCondition )
        { cerr << "no magnetomter measurement" << endl; /* ignore mag measurement */ }
        else
        {
                dXhatPlus = SingleMeasurement( m_magnetometerInertial, (m_measurementsBody)(_(1,3)), dXhatMinus, estimatedAttitude, (m_R)( _(1,3), _(1,3) ) );
        }

        // Update       
        dXhatMinus = dXhatPlus;
        (m_Pminus) = (m_Pplus);
        // Accelerometer Measurement
        dXhatPlus = SingleMeasurement( m_accelerometerInertial, (m_measurementsBody)(_(4,6)), dXhatMinus, estimatedAttitude, (m_R)( _(4,6), _(4,6) ) );

        // Update State Transition Matrix and Covariance Matrix
        StateCovarianceUpdate( dXhatPlus );

        return; 
}       

Vector QuaternionEKFObserver::SingleMeasurement( Vector _inertial, Vector _measurement, Vector _dXhatMinus, Matrix _estimatedAttitude, Matrix _sensorNoise )
{
        // Estimate Output (3x1)
        Vector h(3);
        h( _(1,3) ) = _estimatedAttitude * (_inertial);
        // Determine Sensitivity matrix (3x6)
        Matrix H(3,6);
        H( _(1,3),_(1,3) ) = skew( h ); 

        // Determine Gain Matrix (6x3)  
        Matrix K(6,3);
        Matrix _temp(3,3);
        _temp = ( H*(m_Pminus)*~(H) + _sensorNoise );
        Matrix denominator(3,3);
        denominator = matrixInv3x3( _temp ) ; // denominator for the kalman gain (K)
        K = (m_Pminus)*~(H)*denominator;

        // Update Covariance ( 6x6 )
        m_Pplus = ( eye(6) - K*H )*(m_Pminus);
        //m_Pplus = ( CAMdoubleMatrix::identity(6) - K*H )*(m_Pminus);

        // Determine Residual (3x1)
        Vector E(3);
        E = ( _measurement - h );

        // Update State (6x1)
        Vector dXhatPlus(6);
        dXhatPlus = _dXhatMinus + K*( E-H*_dXhatMinus );
        
        return ( dXhatPlus );
}

        
// update error covariance and state
void QuaternionEKFObserver::StateCovarianceUpdate( Vector dXhatPlus )
{
        Vector dAlphaPlus(3);
        dAlphaPlus = dXhatPlus( _(1,3) );
        Vector dBetaPlus(3);
        dBetaPlus = dXhatPlus( _(4,6) );

        // Determine qplus (output state)
        Matrix Xi( 4,3 );
        Xi( _(1,3), _ ) = ( (m_qminus)(4)*CAMdoubleMatrix::identity(3)+skew ( (m_qminus)( _(1,3) ) ) );
        Xi(4,_) = -(~((m_qminus)(_(1,3))));
        m_qplus = ( m_qminus + 0.5*Xi*dAlphaPlus );
        normalize( m_qplus ); // do a forced normalization

        // set bias
        Vector biasPlus(3);
        biasPlus = m_BM + dBetaPlus;
        m_BM = biasPlus;

        // get new angular rates (output state)
        m_angularRatehat = m_angularRates - biasPlus;

        // Lock output state vector
        pthread_mutex_lock(&m_Mutex);

                // set output state vector
                (m_outputState)( _(1,4) ) = m_qplus;
                (m_outputState)( _(5,7) ) = m_angularRates;
                //(m_outputState)( _(5,7) ) = m_angularRatehat; // might be returning bad result

        // Unlock output state vector
        pthread_mutex_unlock(&m_Mutex);
        
        // Process noise matrix
        Matrix Q(6,6);
        Q(_(1,3),_(1,3)) = ( pow( m_sigmav, 2 )*m_stepTime.GetSeconds( ) + (1/3) *
                pow( m_sigmau, 2 ) * pow( m_stepTime.GetSeconds( ),3 ) )*CAMdoubleMatrix::identity(3);
        Q(_(1,3),_(4,6)) = (0.5 * pow( m_sigmau, 2 ) *
                pow( m_stepTime.GetSeconds( ), 2 ) ) * CAMdoubleMatrix::identity(3);
        Q(_(4,6),_(1,3)) = (0.5 * pow( m_sigmau, 2 ) *
                pow( m_stepTime.GetSeconds( ),2 ))*CAMdoubleMatrix::identity(3);
        Q(_(4,6),_(4,6)) = (pow( m_sigmau, 2 )*m_stepTime.GetSeconds( ))*CAMdoubleMatrix::identity(3);

        StateTransitionMatrix( );

        Matrix Gm(6,6);
        Gm(_(1,3),_(1,3)) = -CAMdoubleMatrix::identity(3);
        Gm(_(4,6),_(4,6)) = CAMdoubleMatrix::identity(3);
        // propagate Pminus
        m_Pminus = m_phi*(m_Pplus)*~(m_phi) + Gm*Q*~Gm;
                
        return;
}

// State Transition Matrix
void QuaternionEKFObserver::StateTransitionMatrix( )
{
        Matrix phi11(3,3);
        Matrix phi12(3,3);
        Matrix phi21(3,3);
        Matrix phi22(3,3);

        double angratemag = norm2( m_angularRatehat );
        
        if (angratemag == 0) // angular rates = 0
        {
                phi11 = CAMdoubleMatrix::identity(3);
                phi12 = -CAMdoubleMatrix::identity(3)*m_stepTime.GetSeconds( );
                phi22 = CAMdoubleMatrix::identity(3);

                (m_qminus) = (m_qplus); // state does not change
        }
        else // rates not equal to zero
        {
                Matrix omegaBar(4,4);

                Vector psiPlus(3);
                psiPlus = sin( 0.5*angratemag*m_stepTime.GetSeconds( ) ) * (m_angularRatehat) /
                    angratemag;
                        
                omegaBar(_(1,3),_(1,3)) = cos( 0.5*angratemag*m_stepTime.GetSeconds( ) ) *
                                CAMdoubleMatrix::identity(3) - skew(psiPlus);
                omegaBar(_(1,3),4) = psiPlus;
                omegaBar(4, _(1,3)) = -~psiPlus;
                omegaBar(4,4) = cos( 0.5*angratemag*(m_stepTime.GetSeconds( ) ) );
                        
                // propagate qminus
                m_qminus = omegaBar*(m_qplus);

                phi11 = CAMdoubleMatrix::identity(3) - skew( m_angularRatehat ) * sin( angratemag*m_stepTime.GetSeconds( ) ) /
                                angratemag+
                                skew( m_angularRatehat ) * skew( m_angularRatehat ) *
                                ( 1.0 - cos( angratemag*m_stepTime.GetSeconds( ) )) /
                                pow( angratemag, 2 );

                phi12 = skew( m_angularRatehat ) * (1.0 - cos( angratemag*m_stepTime.GetSeconds( ))) /
                                pow( angratemag, 2 ) - CAMdoubleMatrix::identity(3)*m_stepTime.GetSeconds( ) -
                                skew( m_angularRatehat ) * skew( m_angularRatehat ) *
                                ( angratemag*m_stepTime.GetSeconds( ) -
                                sin( angratemag*m_stepTime.GetSeconds( ) )) /
                                pow( angratemag,3);

                phi22 = CAMdoubleMatrix::identity(3);

        }

        // Set State Transition Matrix
        (m_phi)( _(1,3),_(1,3) ) = phi11;       (m_phi)( _(1,3),_(4,6) ) = phi12;
        (m_phi)( _(4,6),_(1,3) ) = phi21;       (m_phi)( _(4,6),_(4,6) ) = phi22;

        return;
}                
        
int QuaternionEKFObserver::Run( )
{

        // Lock output state vector
        pthread_mutex_lock(&m_Mutex);
                
                m_whorl->SetState( m_outputState );
        
        // Unlock output state vector
        pthread_mutex_unlock(&m_Mutex);

        return( 0 );
}

//////////////
/////////////
/////////////

void* QuaternionEKFOriginal( void* arg )
{
        QuaternionEKFObserver *EKFObserver = (QuaternionEKFObserver*) arg;

        //cout << "Set State: " << pthread_setcancelstate(PTHREAD_CANCEL_ENABLE,NULL) << endl;
        //cout << "Set Type: " << pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,NULL) << endl;

        // Initialize Filter Parameters
        EKFObserver->InitializeFilter( );

        // Get Current Measurement
        EKFObserver->GetCurrentMeasurements( );

        EKFObserver->m_previousMeasurementTime = EKFObserver->m_currentMeasurementTime;

        EKFObserver->m_stepTime = EKFObserver->m_currentMeasurementTime - EKFObserver->m_previousMeasurementTime;

        EKFObserver->m_durationTime += EKFObserver->m_stepTime.GetSeconds( );

        // Get initial Guess from Triad Algorithm
        EKFObserver->GetInitialGuessTriad( );

        EKFObserver->MeasurementUpdate( );

        // Update
        EKFObserver->StateCovarianceUpdate( );

        pthread_testcancel( );

        EKFObserver->m_previousMeasurement = EKFObserver->m_measurementsBody;

        int exit = 1;

        while( exit )
        {
                usleep( 100000 );

                pthread_testcancel( );

                // Get Current Measurement
                EKFObserver->GetCurrentMeasurements( );

                EKFObserver->CheckMagnetometerMeasurement( );

                pthread_testcancel( );

                EKFObserver->m_stepTime = EKFObserver->m_currentMeasurementTime - EKFObserver->m_previousMeasurementTime;

                EKFObserver->m_durationTime += EKFObserver->m_stepTime.GetSeconds( );

                pthread_testcancel( );

                EKFObserver->MeasurementUpdate( );

                // Update State Transition Matrix and Covariance Matrix
                EKFObserver->StateCovarianceUpdate( );

                pthread_testcancel( );

                EKFObserver->m_previousMeasurementTime = EKFObserver->m_currentMeasurementTime; // reset previous time measurement

                pthread_testcancel( );

                pthread_mutex_lock(&EKFObserver->m_Mutex);
                        exit = EKFObserver->m_exitCondition;
                pthread_mutex_unlock(&EKFObserver->m_Mutex);
        }

        return( arg );
}

// update error covariance and state for original method
void QuaternionEKFObserver::StateCovarianceUpdate( )
{
        m_Pplus = ( CAMdoubleMatrix::identity(6) - (m_K)( _, _(m_initialRange,m_finalRange) )*(m_H)( _(m_initialRange,m_finalRange), _ ) )*(m_Pminus);
        Vector dXhatPlus(6);
        dXhatPlus = (m_K)( _, _(m_initialRange,m_finalRange) )*( (m_measurementsBody)( _(m_initialRange,m_finalRange) ) - (m_h)( _(m_initialRange,m_finalRange) ) );
        Vector dAlphaPlus(3);
        dAlphaPlus = dXhatPlus( _(1,3) );
        Vector dBetaPlus(3); 
        dBetaPlus = dXhatPlus( _(4,6) );

        // Determine qplus (output state)
        Matrix Xi( 4,3 );
        Xi( _(1,3), _ ) = ( (m_qminus)(4)*CAMdoubleMatrix::identity(3)+skew ( (m_qminus)( _(1,3) ) ) );
        Xi(4,_) = -(~((m_qminus)(_(1,3))));
        m_qplus = ( m_qminus + 0.5*Xi*dAlphaPlus );
        normalize( m_qplus ); // do a forced normalization

        // set bias
        Vector biasPlus(3);
        biasPlus = m_BM + dBetaPlus;
        m_BM = biasPlus;

        // get new angular rates (output state)
        m_angularRatehat = m_angularRates - biasPlus;

        // Lock output state vector
        pthread_mutex_lock(&m_Mutex);

                // set output state vector
                (m_outputState)( _(1,4) ) = m_qplus;
                (m_outputState)( _(5,7) ) = m_angularRates;
                //(m_outputState)( _(5,7) ) = m_angularRatehat; // might have problem

        // Unlock output state vector
        pthread_mutex_unlock(&m_Mutex);

        // Process noise matrix
        Matrix Q(6,6);
        Q(_(1,3),_(1,3)) = ( pow( m_sigmav, 2 )*m_stepTime.GetSeconds( ) + (1/3) *
                pow( m_sigmau, 2 ) * pow( m_stepTime.GetSeconds( ),3 ) )*CAMdoubleMatrix::identity(3);
        Q(_(1,3),_(4,6)) = (0.5 * pow( m_sigmau, 2 ) *
                pow( m_stepTime.GetSeconds( ), 2 ) ) * CAMdoubleMatrix::identity(3);
        Q(_(4,6),_(1,3)) = (0.5 * pow( m_sigmau, 2 ) *
                pow( m_stepTime.GetSeconds( ),2 ))*CAMdoubleMatrix::identity(3);
        Q(_(4,6),_(4,6)) = (pow( m_sigmau, 2 )*m_stepTime.GetSeconds( ))*CAMdoubleMatrix::identity(3);

        StateTransitionMatrix( );

        Matrix Gm(6,6);
        Gm(_(1,3),_(1,3)) = -CAMdoubleMatrix::identity(3);
        Gm(_(4,6),_(4,6)) = CAMdoubleMatrix::identity(3);
        // propagate Pminus
        m_Pminus = m_phi*(m_Pplus)*~(m_phi) + Gm*Q*~Gm;

        return;
}

void QuaternionEKFObserver::MeasurementUpdate( )
{
        // set estimated attitude DCM
        Matrix estimatedAttitude(3,3);
        estimatedAttitude = DirectionCosineMatrix( (Quaternion)(m_qminus) );
        
        // sensitivity matrix
        (m_H)( _(1,3),_(1,3) ) = skew( estimatedAttitude * (m_magnetometerInertial) );
        (m_H)( _(4,6),_(1,3) ) = skew( estimatedAttitude * (m_accelerometerInertial) );
        

        m_h( _(1,3) ) = estimatedAttitude * (m_magnetometerInertial);
        m_h( _(4,6) ) = estimatedAttitude * (m_accelerometerInertial);

        Matrix denominator = ( (m_H)( _(m_initialRange,m_finalRange), _ )*(m_Pminus)*~((m_H)( _(m_initialRange,m_finalRange), _ )) 
        + (m_R)( _(m_initialRange,m_finalRange), _(m_initialRange,m_finalRange) ) ).inverse( ) ; // denominator for the kalman gain (m_K)
        (m_K)( _, _(m_initialRange,m_finalRange) ) = (m_Pminus)*~(m_H)( _(m_initialRange,m_finalRange), _ )*denominator;

        return;
}



// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: QuaternionEKFObserver.cpp,v $
*       Revision 1.9  2007/08/31 16:08:01  jayhawk_hokie
*       Modified.
*
*       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
*
*
*
*
******************************************************************************/

---