00001 /************************************************************************************************/ 00002 /*! \file ExtendedKalmanFilter.cpp 00003 * \brief The ExtendedKalmanFilter class provides functionality beyond the KalmanFilter class for nonlinear system models. 00004 * \author $Author: rsharo $ 00005 * \version $Revision: 1.4 $ 00006 * \date $Date: 2003/11/01 21:00:14 $ 00007 ************************************************************************************************/ 00008 /*! 00009 * 00010 ************************************************************************************************/ 00011 00012 #include "ExtendedKalmanFilter.h" 00013 #include "EKFfunctions.h" 00014 #include "LKFfunctions.h" 00015 00016 using namespace std; 00017 00018 /*! Default constructor */ 00019 ExtendedKalmanFilter::ExtendedKalmanFilter() { 00020 00021 SetSystemProcessNoiseMatrix(&Qmatrix); 00022 SetMeasurementCovarianceMatrix(&Rmatrix); 00023 SetStateJacobianMatrix(&Fmatrix); 00024 SetMeasurementJacobianMatrix(&Hmatrix); 00025 00026 SetNonlinearStatePropagator(&EKFStatePropagator); 00027 SetCovariancePropagator(&LKFCovarianceMatrixPropagator); 00028 SetKalmanGainCalculation(&LKFCalcualteKalmanGain); 00029 SetStateMeasurementUpdate(&LKFStateMeasurementUpdate); 00030 SetCovarianceMeasurementUpdate(&LKFCovarianceMeasurementUpdate); 00031 SetPropagationStepSize(DEFAULT_PROPAGATION_STEP_SIZE); 00032 00033 }; 00034 00035 ExtendedKalmanFilter::ExtendedKalmanFilter(double propStepSize) { 00036 00037 SetSystemProcessNoiseMatrix(&Qmatrix); 00038 SetMeasurementCovarianceMatrix(&Rmatrix); 00039 SetStateJacobianMatrix(&Fmatrix); 00040 SetMeasurementJacobianMatrix(&Hmatrix); 00041 00042 SetNonlinearStatePropagator(&EKFStatePropagator); 00043 SetCovariancePropagator(&LKFCovarianceMatrixPropagator); 00044 SetKalmanGainCalculation(&LKFCalcualteKalmanGain); 00045 SetStateMeasurementUpdate(&LKFStateMeasurementUpdate); 00046 SetCovarianceMeasurementUpdate(&LKFCovarianceMeasurementUpdate); 00047 SetPropagationStepSize(propStepSize); 00048 00049 }; 00050 00051 /*! Deconstructor */ 00052 ExtendedKalmanFilter::~ExtendedKalmanFilter() { }; 00053 00054 /*! Gets the current estimated measurements */ 00055 Vector ExtendedKalmanFilter::GetEstimatedMeasurements() { return (*ptr_measurementRHSFunc)(this); }; 00056 00057 void ExtendedKalmanFilter::PropagateState() { ptr_nonlinearStatePropagator(this); }; 00058 00059 void ExtendedKalmanFilter::EstimateState() { 00060 00061 PropagateState(); 00062 PropagateCovariance(); 00063 CalculateKalmanGain(); 00064 MeasurementUpdateState(); 00065 MeasurementUpdateCovariance(); 00066 m_timeOfEstimate = m_timeOfMeasurements; 00067 00068 }; 00069 00070 // Do not change the comments below - they will be added automatically by CVS 00071 /***************************************************************************** 00072 * $Log: ExtendedKalmanFilter.cpp,v $ 00073 * Revision 1.4 2003/11/01 21:00:14 rsharo 00074 * changed "Time.h" includes to "utils/Time.h" includes. Also eliminated excess compile warnings. 00075 * 00076 * Revision 1.3 2003/07/01 21:05:25 mavandyk 00077 * Edited default constructor, and added another constructor to set step size values. 00078 * 00079 * Revision 1.2 2003/06/27 22:39:19 mavandyk 00080 * Fixed many typos and bugs. 00081 * 00082 * Revision 1.1 2003/06/24 22:42:19 mavandyk 00083 * Added get functions for the data member and added comments. 00084 * 00085 * Revision 1.4 2003/06/12 16:11:23 mavandyk 00086 * Changed all references to iterative filter to sequential filter for greater clarity. 00087 * 00088 * Revision 1.3 2003/06/11 15:46:16 simpliciter 00089 * Added include statement for IterativeFilter.h. 00090 * 00091 * Revision 1.2 2003/06/11 13:08:13 simpliciter 00092 * Minor changes. 00093 * 00094 * Revision 1.1 2003/06/11 03:04:20 simpliciter 00095 * Initial submission. 00096 * 00097 ******************************************************************************/ 00098
1.3.9.1