src/Utils/RungeKutta.h

Go to the documentation of this file.

/*****************************************************************************
Copyright (C) 2001-2  Andrew J. Turner (aturner@engineer.com)

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
*****************************************************************************/

#ifndef RUNGEKUTTA_H
#define RUNGEKUTTA_H

#include <matrix/Matrix.h>
using namespace std;
using namespace O_SESSAME;

inline Vector RungeKuttaSolve(ExtendedKalmanFilter* ekf)
{
        // Extract information from EKF instance
        Vector initialState = ekf->GetStateVector();
        Vector params       = ekf->GetParameterVector();
        Vector controls     = ekf->GetControlVector();
        double initialTime  = ekf->GetTimeOfEstimate();
        double finalTime    = ekf->GetTimeOfMeasurements();
        double timeStepSize = ekf->GetPropagationStepSize();

        // Calculate the number of steps for the propagation
        int numberOfSteps = 1;
        
        double h = (finalTime - initialTime) / numberOfSteps;
        double t = initialTime;
        int numEqs = initialState.getIndexCount();
        Vector inputs = initialState;
        Vector K1(numEqs);
        Vector K2(numEqs);
        Vector K3(numEqs);
        Vector K4(numEqs);      
        Vector output(numEqs);

        output = initialState;
        
        Vector temp(numEqs);
        //int prevVal = 0; produced a warning (MCV)
        for (int ii = 1; ii <= numberOfSteps; ++ii)
        {       
                K1 = h * ekf->GetStateDot(t, inputs, controls, params);
                temp = inputs + K1 / 2.0;
                K2 = h * ekf->GetStateDot(t + h/2, temp, controls, params);
                temp = inputs + K2 / 2.0;
                K3 = h * ekf->GetStateDot(t + h/2, temp, controls, params);
                temp = inputs + K3;
                K4 = h * ekf->GetStateDot(t + h, temp, controls, params);
                for (int jj = 0; jj < numEqs; ++jj)
                {
                        inputs(jj+1) += (K1(jj+1) + 2.0 * K2(jj+1) + 2.0 *K3(jj+1) + K4(jj+1)) / 6.0;                   
                }
                t = initialTime + ii * h;
                output = inputs;        
        }
        
        return output;
        
};

inline Vector RungeKutta(Vector initialState, Vector params, Vector controls, double initialTime, double finalTime, int numberOfSteps, Vector (*ptr_rhs)(double time, Vector states, Vector controls, Vector params))
{
        
        double h = (finalTime - initialTime) / numberOfSteps;
        double t = initialTime;
        int numEqs = initialState.getIndexCount();
        Vector inputs = initialState;
        Vector K1(numEqs);
        Vector K2(numEqs);
        Vector K3(numEqs);
        Vector K4(numEqs);      
        Vector output(numEqs);

        output = initialState;
        
        Vector temp(numEqs);
        for (int ii = 1; ii <= numberOfSteps; ++ii)
        {       
                K1 = h * ptr_rhs(t, inputs, controls, params);
                temp = inputs + K1 / 2.0;
                K2 = h * ptr_rhs(t + h/2, temp, controls, params);
                temp = inputs + K2 / 2.0;
                K3 = h * ptr_rhs(t + h/2, temp, controls, params);
                temp = inputs + K3;
                K4 = h * ptr_rhs(t + h, temp, controls, params);
                for (int jj = 0; jj < numEqs; ++jj)
                {
                        inputs(jj+1) += (K1(jj+1) + 2.0 * K2(jj+1) + 2.0 *K3(jj+1) + K4(jj+1)) / 6.0;                   
                }
                t = initialTime + ii * h;
                output = inputs;        
        }

        return output;
        
}

#endif /*RUNGEKUTTA_H*/

---