AttitudeObserver.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file AttitudeObserver.cpp
 *  \brief The Attitude observer class inherits the functionality of the observer class and provides
 *         the space for the the actual filter code.
 *  \author     $Author: jayhawk_hokie $
 *  \version $Revision: 1.4 $
 *  \date    $Date: 2007/08/31 16:08:01 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*  
*/
//////////////////////////////////////////////////////////////////////////////////////////////////

#include "AttitudeObserver.h"

Matrix Att_Fmatrix(KalmanFilter* ekf);
Matrix Att_Hmatrix(KalmanFilter* kf);
Matrix Att_Qmatrix(KalmanFilter* kf);
Matrix Att_Rmatrix(KalmanFilter* kf);
Vector Att_stateRHSFunc(double t, Vector initialState, Vector controls, Vector params);
Vector Att_measurementRHSFunc(ExtendedKalmanFilter* ekf);
Vector qdot(double time, Vector state, Vector controls, Vector params);

/*! Initialize the filter */
int AttitudeObserver::Initialize() {
        
        /*! Set up intial kalman filter data */
        Matrix P(3,3);
        P(1,1) = 0.001; P(1,2) = 0.0;   P(1,3) = 0.0;
        P(2,1) = 0.0;   P(2,2) = 0.001; P(2,3) = 0.0;
        P(3,1) = 0.0;   P(3,2) = 0.0;   P(3,3) = 0.001;
        Matrix K(3,3);
        K(1,1) = 0.001; K(1,2) = 0.0;   K(1,3) = 0.0;
        K(2,1) = 0.0;   K(2,2) = 0.001; K(2,3) = 0.0;
        K(3,1) = 0.0;   K(3,2) = 0.0;   K(3,3) = 0.001;
        
        ekf.SetCovarianceMatrix(P);
        ekf.SetKalmanGainMatrix(K);
        
        ekf.SetSystemProcessNoiseMatrix(&Att_Qmatrix);
        ekf.SetMeasurementCovarianceMatrix(&Att_Rmatrix);
        ekf.SetStateJacobianMatrix(&Att_Fmatrix);
        ekf.SetMeasurementJacobianMatrix(&Att_Hmatrix);
        
        ekf.SetStateRHS(&Att_stateRHSFunc);
        ekf.SetMeasurementRHS(&Att_measurementRHSFunc);
        
        Vector b(3);
        for ( int ii = 1; ii <= 100; ii++ ) {
                b(1) = b(1) + m_whorl->GetRateGyro(string("dmu_rg1"))->GetMeasurement().GetAsDouble();
                b(2) = b(2) + m_whorl->GetRateGyro(string("dmu_rg2"))->GetMeasurement().GetAsDouble();
                b(3) = b(3) + m_whorl->GetRateGyro(string("dmu_rg3"))->GetMeasurement().GetAsDouble();
        };
        
        Vector zeroVector(3);
        m_b = zeroVector;               
        m_b = b / 100;
        
        return 0;
        
};

/*! Run is the funtion that implements the estimator in the derived class Filter */
int AttitudeObserver::Run() {
        
        /** Get rate gyro measurements */
        m_measurements[0] = m_whorl->GetRateGyro(string("dmu_rg1"))->GetMeasurement();
        m_measurements[1] = m_whorl->GetRateGyro(string("dmu_rg2"))->GetMeasurement();
        m_measurements[2] = m_whorl->GetRateGyro(string("dmu_rg3"))->GetMeasurement();
        Vector z(3);
        z(1) = m_measurements[0].GetAsDouble() - m_b(1);
        z(2) = m_measurements[1].GetAsDouble() - m_b(2);
        z(3) = m_measurements[2].GetAsDouble() - m_b(3);
        
        /** Perform time calculations */
        timeval measurementTime, currentTime;
        m_measurements[0].GetTime(measurementTime);
        currentTime = m_whorl->GetTimeOfEstimate();
        double relTimeOfMeasurement = static_cast<double>(measurementTime.tv_sec - currentTime.tv_sec) + (static_cast<double>(measurementTime.tv_usec - currentTime.tv_usec)) * 1e-6;
        
        /* Begin filter code --------------------------------- */
        
        /** Initialize filter */
        Vector state   = m_whorl->GetOmegaBL();
        Vector control = m_whorl->GetControl();
        Vector params  = m_whorl->GetParameter();
        double zero = 0.0;
        ekf.SetTimeOfEstimate(zero);             // set time of the last estimate
        ekf.SetTimeOfMeasurements(relTimeOfMeasurement); // set time of the measurements
        ekf.SetStateVector(state);                  // set the state vector
        ekf.SetControlVector(control);
        ekf.SetParameterVector(params);
        ekf.SetMeasurementVector(z);
        
        /*! Estimate state using measurements */
        ekf.EstimateState();
        
        /* End filter code --------------------------------- */
        // Update the global Whorl class
        state = ekf.GetStateVector();
        Vector fullState(7);
        int numberOfSteps = 5;
        Vector q = m_whorl->GetQuaternion();
        // FIX
        // Unknown symbol: RungeKutta
        // fullState(_(1,4)) = RungeKutta(q, state, control, 0.0, relTimeOfMeasurement, numberOfSteps, &qdot);
        fullState(_(5,7)) = state;

        cout << "RATE? : " << fullState << endl;

        m_whorl->SetState(fullState);
        m_whorl->SetTimeOfEstimate(measurementTime);
        
        return 0;
        
};

Vector qdot(double time, Vector q, Vector controls, Vector w) {
        
        Matrix Q(4,3);
        Q(_(1,3),_(1,3)) = skew(q(_(1,3))) + q(4) * eye(3);
        Q(4,1) = -q(1);
        Q(4,2) = -q(2);
        Q(4,3) = -q(3);
        
        return 0.5 * Q * w;
        
};

Matrix Att_Fmatrix(KalmanFilter* ekf) {
        
        Vector state  = ekf->GetStateVector();
        Vector params = ekf->GetParameterVector();
        
        Matrix MOI(3,3);

        MOI(1,1) = params(1); MOI(1,2) = params(2); MOI(1,3) = params(3);
        MOI(2,1) = params(2); MOI(2,2) = params(4); MOI(2,3) = params(5);
        MOI(3,1) = params(3); MOI(3,2) = params(5); MOI(3,3) = params(6);
        
        Vector w = state;
        
        Matrix temp = MOI/( -skew(w)*MOI +  skew(MOI * w) );
        
        return temp;
        
};

Matrix Att_Hmatrix(KalmanFilter* kf) {
        
        Matrix temp(3,3);
        
        temp(1,1) = 1.0; temp(1,2) = 0.0; temp(1,3) = 0.0;
        temp(2,1) = 0.0; temp(2,2) = 1.0; temp(2,3) = 0.0;
        temp(3,1) = 0.0; temp(3,2) = 0.0; temp(3,3) = 1.0;
        
        return temp;
        
};

Matrix Att_Qmatrix(KalmanFilter* kf) {
        
        Matrix temp(3,3);
        
        temp(1,1) = 0.0001; temp(1,2) = 0.0;   temp(1,3) = 0.0;
        temp(2,1) = 0.0;   temp(2,2) = 0.001; temp(2,3) = 0.0;
        temp(3,1) = 0.0;   temp(3,2) = 0.0;   temp(3,3) = 0.001;

        return temp;

};

Matrix Att_Rmatrix(KalmanFilter* ekf) {
        
        Matrix temp(3,3);
        double gyro_err = 0.005, gyro_std = gyro_err / pow(2.0, 0.5);
        
        temp(1,1) = pow(gyro_std, 2.0); temp(1,2) = 0.0;                temp(1,3) = 0.0;
        temp(2,1) = 0.0;                temp(2,2) = pow(gyro_std, 2.0); temp(2,3) = 0.0;
        temp(3,1) = 0.0;                temp(3,2) = 0.0;                temp(3,3) = pow(gyro_std, 2.0);
        
        return temp;
        
};

Vector Att_stateRHSFunc(double t, Vector initialState, Vector controls, Vector params) {

        Vector w = initialState;

        Matrix MOI(3,3);

        MOI(1,1) = params(1); MOI(1,2) = params(2); MOI(1,3) = params(3);
        MOI(2,1) = params(2); MOI(2,2) = params(4); MOI(2,3) = params(5);
        MOI(3,1) = params(3); MOI(3,2) = params(5); MOI(3,3) = params(6);
        
        return (MOI / ( (-skew(w) * MOI * w) + controls) );
        
};

Vector Att_measurementRHSFunc(ExtendedKalmanFilter* ekf) { return ekf->GetStateVector(); };

// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: AttitudeObserver.cpp,v $
*       Revision 1.4  2007/08/31 16:08:01  jayhawk_hokie
*       Modified.
*       
*       Revision 1.3  2005/03/16 18:08:14  cakinli
*       *** empty log message ***
*       
*       Revision 1.2  2003/08/27 21:55:43  simpliciter
*       Fixed bugs in RHS and F functions.
*       
*       Revision 1.1  2003/08/18 19:38:37  mavandyk
*       *** empty log message ***
*       
*       Revision 1.10  2003/08/13 23:18:07  mavandyk
*       Altered structure as outline in meeting.
*       
*       Revision 1.9  2003/08/06 21:49:11  mavandyk
*       Added comments fixed class structure and runs with config parsig.
*       
*       Revision 1.8  2003/08/01 11:21:30  mavandyk
*       Changed class structure.
*       
*       Revision 1.7  2003/07/22 21:22:52  mavandyk
*       Fixed bugs.
*       
*       Revision 1.6  2003/07/21 21:53:48  mavandyk
*       Fixed some syntax errors.
*       
*       Revision 1.5  2003/07/14 20:06:48  mavandyk
*       Fixed calls to Whorl class, and handling of time and measurements.
*       
*       Revision 1.4  2003/07/10 19:51:02  mavandyk
*       Fixed time handling.
*       
*       Revision 1.3  2003/07/07 20:47:44  mavandyk
*       Changed the state RHS function to use the skew utility function.
*       
*       Revision 1.2  2003/07/07 20:43:34  mavandyk
*       Fixed the Fmatrix function with JS's derivation of the Jacobian matrix for omegadot.
*       
*       Revision 1.1  2003/07/04 14:12:29  simpliciter
*       Moved from src directory.
*       
*       Revision 1.1  2003/07/02 21:33:42  mavandyk
*       The observer for the demo.
*       
*       Revision 1.1  2003/06/30 16:06:00  nicmcp
*       replaced the controller files with the Controller files
*       
*       Revision 1.3  2003/06/30 15:51:57  nicmcp
*       made some changes to the controller files
*       
*       Revision 1.2  2003/06/30 14:13:35  simpliciter
*       Modified comments, added log blocks.
*       
*       
*
******************************************************************************/

---