slidingModeControlMRPMW.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file slidingModeControlMRPMW.cpp
*  \brief Implementation of the slidingModeControlMRPMW class.
*  \author $Author: jayhawk_hokie $
*  \version $Revision: 1.2 $
*  \date    $Date: 2007/08/31 15:53:26 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*! 
*/
//////////////////////////////////////////////////////////////////////////////////////////////////


#include "slidingModeControlMRPMW.h"

/*! /brief Constructor for slidingModeControlMRPMW
 *
 */
slidingModeControlMRPMW::slidingModeControlMRPMW( )
{
}

/*! /brief Constructor for slidingModeControlMRPMW
 *
 *  @param ptr_whorl
 */
slidingModeControlMRPMW::slidingModeControlMRPMW( Whorl* ptr_whorl )
{
        m_whorl = ptr_whorl;
        int retcode = Initialize( );
        retcode = 0;
}

/*! /brief De-Constructor for slidingModeControlMRPMW
 *
 */
slidingModeControlMRPMW::~slidingModeControlMRPMW( ) 
{

}

/*! /brief Initialize slidingModeControlMRPMW
 *
 */
int slidingModeControlMRPMW::Initialize( ) 
{
        m_controlTorque.initialize(3);

        m_uMax = 1.5; // saturation limit for reaction wheels [N-m]

        /* Set the contorl gain matrix */
        m_gainMRPError.initialize(3,3);
/*      m_gainMRPError(1,1) = 0.05;
        m_gainMRPError(2,2) = 0.05;
        m_gainMRPError(3,3) = 0.005;
*/      m_gainMRPError(1,1) = 0.05;
        m_gainMRPError(2,2) = 0.05;
        m_gainMRPError(3,3) = 0.1;

        m_gainMRPRate.initialize(3,3);
        // semi-auto gain selection
        for ( int i=1; i<4; i++ )
        {
                m_gainMRPRate(i,i) = 4*pow( m_gainMRPError(i,i), 0.5 );
        }
        
        /* Get reaction wheel orientation matrix */
        A = FindA( );

        /* Get reaction wheel spin inertia */
        MOI_sw = FindMOI_sw();

        /* Reference Model */
        Quaternion qri( 0.0, 0.0, 0.0, 1.0 );
        m_quaternionReference(1) = qri(1);
        m_quaternionReference(2) = qri(2);
        m_quaternionReference(3) = qri(3);
        m_quaternionReference(4) = qri(4);
        m_mrpReference = ModifiedRodriguezParameters( qri );
        m_angularRateReference(1) = 0;
        m_angularRateReference(2) = 0.0;
        m_angularRateReference(3) = 0.0; // [rad/s]

        ssfTime time;
        time = Now( );
        m_previousTime = time.GetSeconds( );
        
        m_estimateInertiaInv.initialize(3,3);
        m_estimateInertiaInv(_(1,3),_(1,3)) = matrixInv3x3( m_whorl->GetMOI( ) );
        
        m_normEstimateInertiaInv = norm2( m_estimateInertiaInv );

        m_normEstimateInertia = norm2( m_whorl->GetMOI( ) ); 

        m_estimateB.initialize(3,3);
        m_estimateB(_(1,3),_(1,3)) = matrixInv3x3( m_whorl->GetMOI( ) );

        m_estimateBinv.initialize(3,3);
        m_estimateBinv(_(1,3),_(1,3)) = matrixInv3x3( m_estimateB ); 

        /* Bounding Parameters */

        m_phi.initialize(3);
        m_phi(1) = 0.5; // custom parameter
        m_phi(2) = 0.5; // custom parameter
        m_phi(3) = 0.5; // custom parameter
        /*
        m_phi(1) = 0.016; // custom parameter
        m_phi(2) = 0.016; // custom parameter
        m_phi(3) = 0.016; // custom parameter
        */

        m_eta.initialize(3);
        m_eta(1) = 0.04; // custom parameter
        m_eta(2) = 0.04; // custom parameter
        m_eta(3) = 0.04; // custom parameter
        /*
        m_eta(1) = 0.01; // custom parameter
        m_eta(2) = 0.01; // custom parameter
        m_eta(3) = 0.01; // custom parameter
        */

        m_torqueMax = 0.02; // [N-m]
        
        m_D_I.initialize(3,3); // inertial error bound
        m_D_I(1,1) = 0.35, m_D_I(1,2)=0.05, m_D_I(1,3)=0.05;
        m_D_I(2,1) = 0.05, m_D_I(2,2)=0.35, m_D_I(2,3)=0.05;
        m_D_I(3,1) = 0.05, m_D_I(3,2)=0.05, m_D_I(3,3)=0.35;
        m_normD_I = norm2( m_D_I );

        m_D_w.initialize(3,3);
        m_D_w(1,1) = 0.1, m_D_w(1,2) = 0.088, m_D_w(1,3) = 0.088;       
        m_D_w(2,1) = 0.088, m_D_w(2,2) = 0.1, m_D_w(2,3) = 0.088;       
        m_D_w(3,1) = 0.088, m_D_w(3,2) = 0.088, m_D_w(3,3) = 0.1;       

        //m_D_wprime.initialize(3,3); 
        //m_D_wprime(_(1,3),_(1,3)) = matrixInv3x3( m_whorl->GetMOI( ) ) * m_D_w * m_whorl->GetMOI( ); 

//cout << "m_D_wprime: " << m_D_wprime << endl;
        
        m_D_B.initialize(3,3);
        //m_D_B(_(1,3),_(1,3)) = ~m_D_I + m_D_wprime;
        m_D_B(_(1,3),_(1,3)) = matrixInv3x3( m_whorl->GetMOI( ) ) * ( m_D_w + m_D_I + m_D_I*m_D_w )*m_whorl->GetMOI( );

        m_normD_B = norm2( m_D_B );

        m_D_D.initialize(3,3);
        m_D_D(_(1,3),_(1,3)) = m_D_w;

//cout << "norm2 D_B: " << m_D_B << endl;
        m_normD_D = norm2( m_D_D );

        return( 0 );
}

int slidingModeControlMRPMW::Run()
{

        Vector angularRate(3);
        angularRate = m_whorl->GetOmegaBL( );
/*      angularRate(1) = 0.0021;
        angularRate(2) = -0.0004;
        angularRate(3) = -0.0032;
*/
        Vector MRP(3);
        MRP = m_whorl->GetMRP( );
/*      MRP(1) = 0.001;
        MRP(2) = 0.0130;
        MRP(3) = 0.5262;
*/      

        ssfTime currentTime;
        currentTime = Now( );
        ssfTime deltaTime;
        deltaTime = ( currentTime.GetSeconds( ) - m_previousTime );
        //m_previousTime = currentTime.GetSeconds( ); // update time
        // dont' update time, thus provides duration time in seconds.

 
        /* Desired State. Reference Model */
        //ReferenceModelSC( deltaTime );
        ReferenceTrajectory( deltaTime );

        ModifiedRodriguezParameters mrp;
        mrp.Set( MRP );
        ModifiedRodriguezParameters mrpReference;
        mrpReference.Set( m_mrpReference );

        /* Determine Rotation Matirx */
        Rotation RBN;
        RBN.Set( mrp ); // body relative to inertial frame
        Rotation RRN;
        RRN.Set( mrpReference ); // reference relative to inertial frame
        Rotation RBR; // body relative to reference frame
        RBR = RBN*~RRN;

        /* Error */
        Vector angularRateError(3);
        angularRateError = angularRate - RBR.GetDCM( )*(m_angularRateReference); // angular rate error [rad/s]
        ModifiedRodriguezParameters mrpError;
        mrpError = mrp - mrpReference; // mrp error
        mrpError.Switch( 1 ); // important to activate switch condition
        cout << "MRP: " << mrp << endl;
        cout << "Rate: " << angularRate << endl;
        
        /* Set Whorl Error Values */
        m_whorl->SetMRPError( mrpError );
        m_whorl->SetAngularRateError( angularRateError );

        /* Set Whorl Reference Parameters */
        m_whorl->SetReferenceOmegaBL( m_angularRateReference );
        m_whorl->SetReferenceQuaternion( m_quaternionReference );

       /* Reaction Wheel Speed */
        Vector wheelSpeed(3);
        double meastime;

        m_whorl->GetMomentumWheel("REACTION_X")->GetWheelSpeed( wheelSpeed(1), meastime );
        m_whorl->GetMomentumWheel("REACTION_Y")->GetWheelSpeed( wheelSpeed(2), meastime );
        m_whorl->GetMomentumWheel("REACTION_Z")->GetWheelSpeed( wheelSpeed(3), meastime );

/*
wheelSpeed(1) = 10;
wheelSpeed(2) = -10;
wheelSpeed(3) = 10;
*/
        /* Reaction Wheel Angular Momentum */
        Vector hs(3);
        Vector w(3);
        Matrix ws(1,1);
        for ( int i=1; i<4; i++ )
        {
                w(1) = A(1,i);
                w(2) = A(2,i);
                w(3) = A(3,i);
                ws = ~w*angularRate;
                hs(i) = MOI_sw(i,i) * ( ws(1,1) + wheelSpeed(i) );
        }

        Vector estimatef(3);
        estimatef = -m_estimateInertiaInv*skew( angularRate )*( m_whorl->GetMOI( )*angularRate + A*hs );

//      cout << "I^-1: " << m_estimateInertiaInv << endl;
//      cout << "I: " << m_whorl->GetMOI( ) << endl;

//      cout << "hs: " << hs << endl;
//      cout << "A: " << A << endl;

        ModifiedRodriguezParameters mrpRate;
        AttitudeModels MRPRate;
        MRPRate.MRPKinematic( mrpError, angularRateError, mrpRate );

        /* Equavalent Control  */
        Vector ueq(3);

        ueq = m_estimateBinv*( -estimatef + RBR.GetDCM( )*(m_angularAccelReference)
                - skew( angularRate )*RBR.GetDCM( )*(m_angularRateReference) - m_gainMRPRate*mrpRate 
                - m_gainMRPError*mrpError ); // [N-m]
        
        //cout << m_estimateBinv << endl;
        //cout << estimatef << endl;
        //cout << m_gainMRPRate*mrpRate << endl;
        //cout << m_gainMRPError*mrpError << endl; // [N-m]
        
        //cout << "ueq: " << ueq << endl;

        /* Sliding Variable */
        Vector s(3);
        double dt = 0.5;

        //cerr << "angular error: " << angularRateError << endl;
        //cerr << "mrp error: " << mrpError << endl;

        //cout << "MRP Rate Gain: " << m_gainMRPRate << endl;

        //cout << "MRP Error Gain: " << m_gainMRPError << endl;
        //cerr << "dt: " << dt << endl;

        s = angularRateError + m_gainMRPRate*mrpError + m_gainMRPError*mrpError*dt;
        //cout << "s: " << s << endl;

        /* Saturation Function */
        Vector sat(3);
        sat = Saturation( s, m_phi );
        //cout << "sat: " << sat << endl;
        
        Vector D_s(3);
        D_s = A*hs;
        
        Vector one(3);
        one(1) = 1;
        one(2) = 1;
        one(3) = 1;
        
        Vector F(3);
/*
        double normRate = norm2( angularRate );

        F =  (m_normEstimateInertiaInv)*( 2*m_normD_I + pow( m_normD_I, 2 ) )* m_normEstimateInertia  * pow( normRate,2 )*one 
                + (m_normEstimateInertiaInv)*( m_normD_I + m_normD_D + pow( m_normD_I, 2 )  ) * normRate * norm2( D_s )*one
                + (m_normEstimateInertiaInv)*( 1 + (m_normD_I) )*m_torqueMax*one; 
*/

        F = matrixInv3x3( m_whorl->GetMOI( ) )*Vabs( ( -skew( angularRate )*m_D_I + m_D_I*skew( angularRate ) + m_D_I*skew( angularRate )*m_D_I )*m_whorl->GetMOI( )*angularRate
                + ( -skew( angularRate )*m_D_D + m_D_I*skew( angularRate ) + m_D_I*skew( angularRate )*m_D_D )*D_s
                + ( eye(3) + m_D_I )*m_torqueMax*one  );

cout << "F: " << F << endl;
//cout << "m_normD_D: " << m_normD_D << endl;
//cout << "normRate: " << normRate << endl;
//cout << "Ds: " << D_s << endl;
//cout << "norm2 Ds: " << norm2( D_s ) << endl;
//cout << "1: " << (m_normEstimateInertiaInv)*( m_normD_I + m_normD_D + pow( m_normD_I, 2 )  ) * normRate * norm2( D_s )*one << endl;
//cout << "2: " << (m_normEstimateInertiaInv)*( 1 + (m_normD_I) )*m_torqueMax*one << endl; 
//cout << "3: " << (m_normEstimateInertiaInv)*( 2*m_normD_I + pow( m_normD_I, 2 ) )* m_normEstimateInertia  * pow( normRate,2 )*one << endl;

        /* Robust Control Gain */
        Vector K(3);
        K = matrixInv3x3( eye(3) - m_D_B )*( F + m_D_B*(Vabs( estimatef - RBR.GetDCM( )*(m_angularAccelReference)
                + skew( angularRate )*RBR.GetDCM( )*(m_angularRateReference) + m_gainMRPRate*mrpRate
                + m_gainMRPError*mrpError ) ) + m_eta );

/*cout <<       Vabs( m_D_B*( estimatef - RBR.GetDCM( )*(m_angularAccelReference)
                + skew( angularRate )*RBR.GetDCM( )*(m_angularRateReference) + m_gainMRPRate*mrpRate
                + m_gainMRPError*mrpError ) )   << endl;
*/
        //cout << "K: " << K << endl;

        Matrix matK(3,3);
        matK(1,1) = K(1); 
        matK(2,2) = K(2); 
        matK(3,3) = K(3); 
cerr << "K: " << ~K << endl;
//cerr << "m_estimateBinv: " << m_estimateBinv << endl;
        /* Robust Control  */
        Vector urob(3);
        urob = m_estimateBinv*matK*sat; // [N-m]


        //out << "K: " << K << endl;

        //cout << "urob: " << urob << endl;

        /* Sliding Mode Control Law */
        m_controlTorque = ( ueq - urob ); // [N-m]
        cout << "control before A: " << ~m_controlTorque << endl;
        m_controlTorque = ~A*m_controlTorque; // [N-m]

        cout << "A: " << A << endl;
        cout << "A inv: " << (~A) << endl;
        cout << "eq: " << ~ueq << endl;
        cout << "rob: " << ~urob << endl;

        cout << "control after A: " << ~m_controlTorque << endl;

        m_controlTorque = WheelSaturation( m_controlTorque );
        
        cout << "control after Wheel Sat: " << ~m_controlTorque << endl;
        
        /* Set the desird control torque in the whorl object */
        m_whorl->SetControl( m_controlTorque ); // [N-m]

        /* Set the torque for the momentum wheels to produce */
        SetWheelTorque( m_controlTorque ); // [N-m]

        return( 0 );
}

// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: slidingModeControlMRPMW.cpp,v $
*       Revision 1.2  2007/08/31 15:53:26  jayhawk_hokie
*       Modified.
*       
*       Revision 1.1  2007/07/24 09:25:15  jayhawk_hokie
*       Initial Submission.
*       
*       
*       
*
******************************************************************************/

---