ExtendedKalmanFilter.h

Go to the documentation of this file.

/************************************************************************************************/
/*! \file ExtendedKalmanFilter.h
*  \brief The ExtendedKalmanFilter class provides functionality beyond the KalmanFilter class for nonlinear system models.
*  \author $Author: cakinli $
*  \version $Revision: 1.9 $
*  \date    $Date: 2005/02/25 18:40:53 $
************************************************************************************************/
/*! 
*
************************************************************************************************/

#ifndef __SSSL_EXTENDEDKALMANFILTER_H__
#define __SSSL_EXTENDEDKALMANFILTER_H__

#include <Filtering/SequentialFilter.h>
#include <Filtering/KalmanFilter.h>

/*! Constants */
#define DEFAULT_PROPAGATION_STEP_SIZE 0.01 /*!< default propagation step size in seconds */

using namespace std;

class ExtendedKalmanFilter : public KalmanFilter {
        
public:
        // Contructors/Deconstructors
        
                /*! Default constructor */
                ExtendedKalmanFilter();
        
                /*! Constructor */
                ExtendedKalmanFilter(double propStepSize);

                /*! Deconstructor */
                virtual ~ExtendedKalmanFilter();

        // Facilitators
        
                // Mutators
                        
                        /*! Sets the pointer to the state RHS function */
                        inline void SetStateRHS(Vector (*stateRHSFunc)(double time, Vector states, Vector controls, Vector params)) { ptr_stateRHSFunc = stateRHSFunc; };
                        
                        /*! Set the pointer to the measurment RHS function */
                        inline void SetMeasurementRHS(Vector (*measurementRHSFunc)(ExtendedKalmanFilter*)) { ptr_measurementRHSFunc = measurementRHSFunc; };
                        
                        /*! Sets the propagation step size */
                        inline void SetPropagationStepSize(double stepSize) { m_propagationStepSize = stepSize; };
                        
                        /*! Sets the nonlinear state propagation */
                        inline void SetNonlinearStatePropagator(void (*statePropagateFunc)(ExtendedKalmanFilter*)) { ptr_nonlinearStatePropagator = statePropagateFunc; }
                        
                        /*! Estimate the state with current filter information */
                        virtual void EstimateState();
                        
                // Inspectors
                        
                        /*! Gets the current 1st time derivative of the state */
                        inline Vector GetStateDot(double t, Vector states, Vector controls, Vector params) { return (*ptr_stateRHSFunc)(t, states, controls, params); };
                        
                        /*! Gets the propagation step size */
                        inline double GetPropagationStepSize() { return m_propagationStepSize; };
                                
                        /*! Gets the current estimated measurements */
                        Vector GetEstimatedMeasurements();

protected:
        
        Vector (*ptr_stateRHSFunc)(double time, Vector states, Vector controls, Vector params); /*!< A function pointer to the (nonlinear) right-hand side function for the states, \f$\dot{x} = f(x,u,w)\f$. */
        Vector (*ptr_measurementRHSFunc)(ExtendedKalmanFilter*); /*!< A function pointer to the measurement right-hand side file, \f$ z = h(x,u,w)\f$. */
        void (*ptr_nonlinearStatePropagator)(ExtendedKalmanFilter*);
        double m_propagationStepSize;
                        
        void PropagateState();
                                
private:

};



#endif 
// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
* $Log: ExtendedKalmanFilter.h,v $
* Revision 1.9  2005/02/25 18:40:53  cakinli
* Created Makefiles and organized include directives to reduce the number of
* include paths.  Reorganized libraries so that there is now one per source
* directory.  Each directory is self-contained in terms of its Makefile.
* The local Makefile in each directory includes src/config.mk, which has all
* the definitions and general and pattern rules.  So at most, to see what
* goes into building a target, a person needs to examine the Makefile in
* that directory, and ../config.mk.
*
* Revision 1.8  2003/07/01 21:05:39  mavandyk
* Edited default constructor, and added another constructor to set step size values.
*
* Revision 1.7  2003/06/30 21:08:43  mavandyk
* Removed get pointer to state RHS function and inserted a member function to calculate the first time derivative of the states.
*
* Revision 1.6  2003/06/27 22:39:05  mavandyk
* Fixed many typos and bugs.
*
* Revision 1.5  2003/06/24 22:41:35  mavandyk
* Added get functions for the data member and added comments.
*
* Revision 1.4  2003/06/12 16:11:23  mavandyk
* Changed all references to iterative filter to sequential filter for greater clarity.
*
* Revision 1.3  2003/06/11 15:46:16  simpliciter
* Added include statement for IterativeFilter.h.
*
* Revision 1.2  2003/06/11 13:08:13  simpliciter
* Minor changes.
*
* Revision 1.1  2003/06/11 03:04:20  simpliciter
* Initial submission.
*
******************************************************************************/

---