DSACSS Operational Code: IteratedExtendedKalmanFilter.cpp Source File

00001 /************************************************************************************************/
00002 /*! \file IteratedExtendedKalmanFilter.cpp
00003 *  \brief The IteratedExtendedKalmanFilter class provides functionality beyond the ExtendedKalmanFilter class for advanced calculation of the measurement update of the state.
00004 *  \author $Author: simpliciter $
00005 *  \version $Revision: 1.7 $
00006 *  \date    $Date: 2003/07/14 20:47:45 $
00007 ************************************************************************************************/
00008 /*! 
00009 *
00010 ************************************************************************************************/
00011 
00012 #include "IteratedExtendedKalmanFilter.h"
00013 #include "LKFfunctions.h"
00014 #include "EKFfunctions.h"
00015 
00016 IteratedExtendedKalmanFilter::IteratedExtendedKalmanFilter() {
00017         
00018         SetSystemProcessNoiseMatrix(&Qmatrix);
00019         SetMeasurementCovarianceMatrix(&Rmatrix);
00020         SetStateJacobianMatrix(&Fmatrix);
00021         SetMeasurementJacobianMatrix(&Hmatrix);
00022         
00023         SetNonlinearStatePropagator(&EKFStatePropagator);
00024     SetCovariancePropagator(&LKFCovarianceMatrixPropagator);
00025     SetKalmanGainCalculation(&LKFCalcualteKalmanGain);
00026     SetStateMeasurementUpdate(&LKFStateMeasurementUpdate);
00027     SetCovarianceMeasurementUpdate(&LKFCovarianceMeasurementUpdate);
00028         SetPropagationStepSize(DEFAULT_PROPAGATION_STEP_SIZE);
00029         
00030         SetIterationLimit(DEFAULT_ITERATION_LIMIT);
00031         SetTolerance(DEFAULT_TOLERANCE);
00032         
00033 };
00034 
00035 IteratedExtendedKalmanFilter::IteratedExtendedKalmanFilter(double propStepSize, int iterLimit, double tol) {
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         SetIterationLimit(iterLimit);
00050         SetTolerance(tol);
00051         
00052 };
00053 
00054 /*! Estimates the states at the time of the measurements */
00055 void IteratedExtendedKalmanFilter::EstimateState() {
00056         
00057         PropagateState();
00058         PropagateCovariance();
00059         CalculateKalmanGain();
00060         MeasurementUpdateState();
00061         
00062         int count = 0;
00063         Vector stateError = m_states;
00064         Vector priorStates = m_states;
00065         while ((count <= m_iterationLimit) && (norm2(stateError) > m_tolerance)) {
00066                 
00067                 count++;
00068                 CalculateKalmanGain();
00069                 MeasurementUpdateState();
00070                 stateError = m_states - priorStates;
00071                 priorStates = m_states;
00072                                 
00073         };
00074         
00075         MeasurementUpdateCovariance();
00076         m_timeOfEstimate = m_timeOfMeasurements;
00077         m_numIterations = count;
00078         
00079 };
00080 
00081 // Do not change the comments below - they will be added automatically by CVS
00082 /*****************************************************************************
00083 * $Log: IteratedExtendedKalmanFilter.cpp,v $
00084 * Revision 1.7  2003/07/14 20:47:45  simpliciter
00085 * Fixed logic error in looping.
00086 *
00087 * Revision 1.6  2003/07/14 19:42:49  simpliciter
00088 * Cleaner code.
00089 *
00090 * Revision 1.5  2003/07/14 19:36:27  simpliciter
00091 * Fixed iteration bug
00092 *
00093 * Revision 1.4  2003/07/12 17:57:52  simpliciter
00094 * Added IEKF and EKF Histories.
00095 *
00096 * Revision 1.3  2003/07/01 21:04:54  mavandyk
00097 * Edited default constructor, and added another constructor to set iteration and step size values.
00098 *
00099 * Revision 1.2  2003/07/01 20:24:01  mavandyk
00100 * Corrected syntax errors.
00101 *
00102 * Revision 1.1  2003/06/30 21:49:01  mavandyk
00103 * Implementation file for the IteratedExtendedKalmanFilter class.
00104 *
00105 * Revision 1.4  2003/06/12 16:16:00  mavandyk
00106 * Changed all references to iterative filter to sequential filter for greater clarity.  Removed improper lines of code involving iterated filter references.
00107 *
00108 * Revision 1.3  2003/06/11 15:46:42  simpliciter
00109 * Added include line for IterativeFilter.h.
00110 *
00111 * Revision 1.2  2003/06/11 13:08:27  simpliciter
00112 * Minor changes.
00113 *
00114 * Revision 1.1  2003/06/11 03:04:43  simpliciter
00115 * Initial submission.
00116 *
00117 ******************************************************************************/