EKFomegaEKFparam.cpp

Go to the documentation of this file.

/************************************************************************************************/
/*! \file EKFomegaEKFparam.cpp
*  \brief A dual EKF to estimate the angular velocities and moment of inertia 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>
#include <float.h>
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);

Matrix param_Euler_Ematrix(ExtendedKalmanFilter* stateEKF);  // analogous to state H matrix


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.2;
        states(2) = 0.2;
        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/EKFomegaEKFparam.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);
        
        
        
        /*! Declare and initalize parameter filter and its history */
        double forgettingFactor = 0.999;        // controls, um, everything?
        Matrix param_covariance = eye(6);
        param_covariance *= 0.5;        // controls initial behavior
                
        Matrix param_kalmanGain(6,3);
        Matrix param_Rmatrix(3,3);
        
        Matrix Ek;
        Vector errorVector;
        Vector previousStates;
        Vector newParameters;
        
        
        
        
        /* 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();
                previousStates = states;
                states = state_ekf.GetStateVector();
                
                
                /*! Perform param_ekf update */
                param_covariance *= 1.0/forgettingFactor;

                Ek = param_Euler_Ematrix(&state_ekf);
                
                param_Rmatrix = eye(3) * pow( (0.005/pow(2.0,0.5)), 2.0 );      // controls decay rate, fiddle with the first number
                param_kalmanGain = (param_covariance * ~Ek) * (Ek * param_covariance * ~Ek + param_Rmatrix).inverse();

                
                errorVector = (( states - previousStates ) / state_ekf.GetPropagationStepSize()) 
                                                         - state_ekf.GetStateDot(t, states, controls, parameters);
                
                newParameters = parameters + param_kalmanGain*errorVector;                              
                param_covariance = ( eye(6) - param_kalmanGain*Ek )*param_covariance;


                /*! Update the state_ekf parameter data. */
                state_ekf.SetParameterVector(newParameters);
                
                
                /*! 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;
        
};







Matrix param_Euler_Ematrix(ExtendedKalmanFilter* stateEKF) {
        
        Matrix  temp(3,6);
        double  t = stateEKF->GetTimeOfEstimate();
        Vector  state = stateEKF->GetStateVector();
        Vector  controls = stateEKF->GetControlVector();
        Vector  params = stateEKF->GetParameterVector();
        Vector  twiddleParams;
        Vector  twiddlef;
        
        Vector  nominalf;
        nominalf = stateEKF->GetStateDot(t, state, controls, params);
        
        double  delta = sqrt(DBL_EPSILON);
        double  perturbation;
        
                        
        for (int jj = 1; jj <= 6; jj++) {
                // temp( _(1,3),jj )
                twiddleParams = params;
                perturbation = twiddleParams(jj)*delta + delta;  // + delta avoids problem of twiddleParams(jj) = 0
                twiddleParams(jj) += perturbation;
        
                twiddlef = stateEKF->GetStateDot(t, state, controls, twiddleParams);
                temp( _(1,3),jj ) = (twiddlef - nominalf) / perturbation;
        }
        
        return temp;
        
};













// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
* $Log: EKFomegaEKFparam.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:07  simpliciter
* Good gains.
*
* Revision 1.1  2003/07/10 20:17:45  simpliciter
* These work.
*
*
******************************************************************************/

---