triadObserver.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file triadObserver.cpp
*  \brief Observes attitute with the triad algorithm
*  \author $Author: jayhawk_hokie $
*  \version $Revision: 1.6 $
*  \date    $Date: 2007/08/31 16:08:01 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*!
*/
//////////////////////////////////////////////////////////////////////////////////////////////////


#include "triadObserver.h"

using namespace std;

/*! \brief Default Constructor */
triadObserver::triadObserver( )
{ 
        Initialize( );
}

/*! \brief Create a triad 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
 */
triadObserver::triadObserver( TiXmlHandle handle, Whorl* ptr_whorl )
{
        m_whorl = ptr_whorl;
        
        Initialize( );

        // call parse function to get inertial measurement vectors
        Parse( handle );
}

/*! \brief DeConstructor for triadObserver object */
triadObserver::~triadObserver( )
{ }

/*! \brief Initialize triad observer
*/
int triadObserver::Initialize( )
{
        m_magBody.initialize(3);

        m_2magBodyPrevious.initialize(3);

        m_accelBody.initialize(3);
        m_accelInertial.initialize(3);
        m_magInertial.initialize(3);
        m_ratesBody.initialize(3);
        m_AngularAccel.initialize(3);

        m_rsensor.initialize(3);
        
        return( 0 );
}

/*! \brief Parse inertial measurement vectors from configuration file
        * @param handle XML handle with current whorl as starting node
        */
void triadObserver::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_magInertial)(1) );
        checkResponse(response);
        response = magElement.Child("INERTIAL_VECTOR", 0).Element() ->  QueryDoubleAttribute( "valueY", &(m_magInertial)(2) );
        checkResponse(response);
        response = magElement.Child("INERTIAL_VECTOR", 0).Element() ->  QueryDoubleAttribute( "valueZ", &(m_magInertial)(3) );
        checkResponse(response);

        // Normalize magnetmeter inertial vector
        normalize( m_magInertial );

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

        // Normalize accelerometer inertial vector
        normalize( m_accelInertial ); // [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);
        }
}
        
/*! \brief Take measurements and update state vector in whorl object */
int triadObserver::Run( )
{
        //static double XaccelCorr, YaccelCorr, ZaccelCorr; // acceleration correction variables
        //static double rX = -14.25; // distance from center of rotation to motionpak sensors in inches (x-direction)
        DirectionCosineMatrix Rbi;
        Quaternion attitude;
        Vector correction(3);
        Vector state(7);  // state = [q_bar; q4; rates]

        /* since the magnetometer, accelerometers and rate gyros are all we're using for now, this function assumes their use
         and ONLY their use*/

        // get accelerometer reading

        (m_accelBody)(1) = m_whorl->GetAccelerometer("ACC_X")->GetMeasurement().GetAsDouble();
        (m_accelBody)(2) = m_whorl->GetAccelerometer("ACC_Y")->GetMeasurement().GetAsDouble();
        (m_accelBody)(3) = m_whorl->GetAccelerometer("ACC_Z")->GetMeasurement().GetAsDouble();

        // get magnetomter reading

        m_magBody = m_whorl->GetMagnetometer()->GetVectorMeasurement();

        if( fabs( norm2(m_magBody) - norm2(m_2magBodyPrevious) ) > 70 )
        {
                m_magBody = m_whorl->GetMagnetometer()->GetVectorMeasurement();
                m_2magBodyPrevious = m_magBody;
        }
        else
        {
                m_2magBodyPrevious = m_magBody;
        }       


        // get rate gyro reading

        (m_ratesBody)(1) = m_whorl->GetRateGyro("RATE_GYRO_X")->GetMeasurement().GetAsDouble();
        (m_ratesBody)(2) = m_whorl->GetRateGyro("RATE_GYRO_Y")->GetMeasurement().GetAsDouble();
        (m_ratesBody)(3) = m_whorl->GetRateGyro("RATE_GYRO_Z")->GetMeasurement().GetAsDouble();
        
        // Have no sensor for determing angular acceleration. Need to Address this issue.
        Vector angularAcceleration(3);

        // Correct Acceleration vector due to rotation
        AccelerationCorrection( angularAcceleration, m_ratesBody, m_accelBody, m_rsensor );

        // perfrorm triad algorithm on measurements and extract quaternion
        Rbi = GetAttitude( );

        attitude = Rbi.GetQuaternion();

        // set state vector
        state(_(1,4)) = attitude;
        state(_(5,7)) = m_ratesBody;

        m_whorl->SetState( state );

        return 0;
}

/*! \brief Triad algorithm implementation (called from run function)
        * @return Rbi 3x3 Attitude matrix 
        */
Matrix triadObserver::GetAttitude( )
{
        
        Vector t1b(3), t2b(3), t3b(3), t1i(3), t2i(3), t3i(3); // triad vectors
        Matrix Rbt(3,3), Rti(3,3); //triad rotation matrices
        Matrix Rbi;     
        
        // normalize measurement vectors
        
        normalize( m_magBody );
        normalize( m_accelBody );

        // create triad vectors

        t1b = m_accelBody;
        t2b = crossP(t1b,m_magBody);    
        normalize(t2b);
        t3b = crossP(t1b,t2b); // does not need to be normalized since theta=90 deg

        
        t1i = m_accelInertial;
        t2i = crossP( t1i, m_magInertial );     
        normalize(t2i);
        t3i = crossP(t1i,t2i);

        // set triad rotation matrices
                
        Rbt(_(1,3),_(1,1)) = t1b;
        Rbt(_(1,3),_(2,2)) = t2b;
        Rbt(_(1,3),_(3,3)) = t3b;
                
        
        Rti(_(1,1),_(1,3)) = ~t1i;
        Rti(_(2,2),_(1,3)) = ~t2i;
        Rti(_(3,3),_(1,3)) = ~t3i;
        
        // set final Rbi matrix
         
        Rbi = Rbt*Rti;  

        return Rbi;
}

void triadObserver::SetMagnetometerBodyVector( Vector _magnetometer )
{
        m_magBody = _magnetometer;
}

void triadObserver::SetMagnetometerInertialVector( Vector _magnetometer )
{
        m_magInertial = _magnetometer;
}

void triadObserver::SetAccelerometerBodyVector( Vector _accelerometer )
{
        m_accelBody = _accelerometer;
}

void triadObserver::SetAccelerometerInertialVector( Vector _accelerometer )
{
        m_accelInertial = _accelerometer;
}



// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: triadObserver.cpp,v $
*       Revision 1.6  2007/08/31 16:08:01  jayhawk_hokie
*       Modified.
*
*       Revision 1.5  2007/07/24 08:45:27  jayhawk_hokie
*       Replaced matrix pointers.
*
*       Revision 1.4  2007/05/29 19:02:15  jayhawk_hokie
*       *** empty log message ***
*
*       Revision 1.3  2007/03/27 19:15:44  jayhawk_hokie
*       Updated Class.
*
*       Revision 1.2  2007/03/21 20:12:28  jayhawk_hokie
*       Update of the Triad Observer Class
*
*       Revision 1.1  2007/01/12 22:11:57  bwilliam
*       Initial submission.
*
******************************************************************************/

---