EKFomega.cpp

Go to the documentation of this file.

/************************************************************************************************/
/*! \file EKFomega.cpp
*  \brief An EKF to estimate the angular velocities in the simple form of Euler's Eqns.
*  \author $Author: rsharo $
*  \version $Revision: 1.3 $
*  \date    $Date: 2003/11/01 21:00:17 $
************************************************************************************************/
/*! 
*
************************************************************************************************/

#include "ExtendedKalmanFilter.h"
#include "KalmanFilterHistory.h"
#include "utils/Time.h"
#include <iostream>
#include <fstream>
using namespace std;
using namespace O_SESSAME;

Matrix state_Euler_Fmatrix(KalmanFilter*);
Matrix state_Euler_Hmatrix(KalmanFilter*);
Matrix state_Euler_Qmatrix(KalmanFilter*);
Matrix state_Euler_Rmatrix(KalmanFilter*);

Vector state_Euler_stateRHSFunc(double t, Vector initialState, Vector controls, Vector params);
Vector state_Euler_measurementRHSFunc(ExtendedKalmanFilter* ekf);

int main() {
        
        tick();         // start the timer.
                
        /*! Declare and initialize intermediate variables */
        Vector states(3);
        Vector parameters(6);
        Vector measurements(3);
        Vector controls(3);                     // uninitialized:  remains a zero Vector for the open-loop case.
        double t = 0.0;
        
        /*! Set intial conditions */
        states(1) = 0.25;
        states(2) = 0.3;
        states(3) = 0.2;
        
        parameters(1) = 5.7;
        parameters(2) = 0.4;
        parameters(3) = 0.3;
        parameters(4) = 5.3;
        parameters(5) = 0.45;
        parameters(6) = 9.7;
                
        
        /*! Prepare data files */
        ifstream iFile;
        ofstream oFile;
        
        /*! Open input and output files */
        iFile.open("DataFiles/eulermeasurements.txt");
        oFile.open("DataFiles/EKFomega_bestguess.txt");
        
        
        /*! Read in initial measurements */
        iFile >> t >> measurements(1) >> measurements(2) >> measurements(3);



        /*! Set up intial kalman filter data */
        Matrix covariance = eye(3);
        covariance *= 0.5;
        
        Matrix kalmanGain(3,3); // does not need to be initialized.
        
        
        /*! Declare and initalize state filter and history */
        ExtendedKalmanFilter state_ekf;
        KalmanFilterHistory state_ekfhist(&state_ekf);
        
        state_ekf.SetCovarianceMatrix(covariance);
        state_ekf.SetKalmanGainMatrix(kalmanGain);
        
        state_ekf.SetTimeOfEstimate(t);
        state_ekf.SetTimeOfMeasurements(t);
        state_ekf.SetStateVector(states);
        state_ekf.SetControlVector(controls);
        state_ekf.SetMeasurementVector(measurements);
        state_ekf.SetParameterVector(parameters);

        state_ekf.SetSystemProcessNoiseMatrix(&state_Euler_Qmatrix);
        state_ekf.SetMeasurementCovarianceMatrix(&state_Euler_Rmatrix);
        state_ekf.SetStateJacobianMatrix(&state_Euler_Fmatrix);
        state_ekf.SetMeasurementJacobianMatrix(&state_Euler_Hmatrix);
        
        state_ekf.SetStateRHS(&state_Euler_stateRHSFunc);
        state_ekf.SetMeasurementRHS(&state_Euler_measurementRHSFunc);
        
        state_ekfhist.AppendHistory(t);
        
        
        /* Run the loop! */
                
        while (iFile) {
                
                /*! Get next measurement */
                iFile >> t >> measurements(1) >> measurements(2) >> measurements(3);

                /*! Set new SF data including new measurements */
                state_ekf.SetTimeOfMeasurements(t);
                state_ekf.SetMeasurementVector(measurements);
                
                /*! Estimate state using measurements */
                state_ekf.EstimateState();
                
                /*! Update the history */
                state_ekfhist.AppendHistory(t);
                
        }
        
        oFile << state_ekfhist.GetHistory();
        
        cout << "Run completed @ " << tock() << endl;
        
        iFile.close();
        oFile.close();
        
        return 1;
        
}






Vector state_Euler_stateRHSFunc(double t, Vector initialState, Vector controls, Vector params) {
        
        // build the moment of inertial matrix from the parameter vector
        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);

        // build angular velocity stuff
        Vector omega(3);
        omega = initialState;
        
        // build updated state vector
        Vector updatedState(3);
        updatedState = MOI/( -skew(omega) * MOI * omega );
                
        return (updatedState);
        
};


/*! Right now, this is providing angular velocity feedback
 * For the one-millionth time, is this what we get from the rate gyros? */
Vector state_Euler_measurementRHSFunc(ExtendedKalmanFilter* ekf) { 
        
        Vector temp(3);
        temp = ekf->GetStateVector(); 
        
        return(temp);
        
};



Matrix state_Euler_Fmatrix(KalmanFilter* ekf) {
        
        Vector state  = ekf->GetStateVector();
        Vector params = ekf->GetParameterVector();
        
        // build the moment of inertial matrix from the parameter vector
        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);
        
        // build angular velocity stuff
        Vector omega(3);
        omega = state;
        
        // build the state Jacobian matrix, df/dx
        Matrix temp(3,3);
        temp =  MOI/( -skew(omega) * MOI + skew(MOI*omega) );
        
        return temp;
        
};

Matrix state_Euler_Hmatrix(KalmanFilter* ekf) {
        
        Matrix temp(3,3);
        temp = eye(3);
        
        return temp;
        
};

Matrix state_Euler_Qmatrix(KalmanFilter* ekf) {
        
        Matrix temp = eye(3);
        temp *= 0.002;

        return temp;

};

Matrix state_Euler_Rmatrix(KalmanFilter* ekf) {
        
        Matrix temp = eye(3);
        double gyro_err = 1.0, gyro_std = gyro_err / pow(2.0, 0.5);
        
        temp *= pow(gyro_std, 2.0);
        
        return temp;
        
};








// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
* $Log: EKFomega.cpp,v $
* Revision 1.3  2003/11/01 21:00:17  rsharo
* changed "Time.h" includes to "utils/Time.h" includes. Also eliminated excess compile warnings.
*
* Revision 1.2  2003/07/11 14:10:06  simpliciter
* Good gains.
*
* Revision 1.1  2003/07/10 20:17:45  simpliciter
* These work.
*
*
******************************************************************************/

---