IteratedExtendedKalmanFilterHistory.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file IteratedExtendedKalmanFilterHistory.cpp
*  \brief Provides functionality beyond ExtendedKalmanFilterHistory to store the number of iterations at each time-step.
 *  \author $Author: rsharo $
 *  \version $Revision: 1.3 $
 *  \date    $Date: 2003/11/01 21:00:14 $
 *//////////////////////////////////////////////////////////////////////////////////////////////////
/*  
 */
//////////////////////////////////////////////////////////////////////////////////////////////////

#include "IteratedExtendedKalmanFilterHistory.h"

/*! \brief Only one constructor.  (No empty constructor.)
 * Builds an empty history for the filter instance passed in as a pointer.  */
IteratedExtendedKalmanFilterHistory::IteratedExtendedKalmanFilterHistory(IteratedExtendedKalmanFilter* _ptr_filter)
{
        // calls the initialization sequence in SequentialFilterHistory
    InitializeSequentialFilterHistory(_ptr_filter);
        
        // new Kalman Filter data to be stored:  the covariance and Kalman gain matrices.
        m_kfCovarianceHistory.reserve(HISTORY_RESERVE_SIZE);
        m_kfKalmanGainHistory.reserve(HISTORY_RESERVE_SIZE);
        
        // new IEKF data to be stored:  the number of iterations for each measurement update.
        m_iekfIterationHistory.reserve(HISTORY_RESERVE_SIZE);
        
        // not doing interpolations at this time
        //m_kalmanFilterInterpolations.reserve(HISTORY_RESERVE_SIZE);
}



/*! \brief Add the filters's data, which occured at a time in seconds, to the history.
 *
 *  Appends the state at t=_appendTime. if the new state 
 *      occured at a time that is earlier than any of the stored values
 *      then the time history will be erased from the overlap point 
 *      and the new state will be appended.
 * @param _appendTime time (in seconds) to be added, t.
 * 
 */
void IteratedExtendedKalmanFilterHistory::AppendHistory(const double& _appendTime)
{
    AppendHistory(ssfTime(_appendTime));
}    



/*! \brief Add the filters's data, which occured at a time in ssfTime form, to the history.
 *
 *  Appends the state at t=_appendTime. if the new state 
 *      occured at a time that is earlier than any of the stored values
 *      then the time history will be erased from the overlap point 
 *      and the new state will be appended.
 * @param _appendTime the ssfTime object specifying when the state occured, t.
 *
 */
void IteratedExtendedKalmanFilterHistory::AppendHistory(const ssfTime& _appendTime)
{
        // This guy logs the time, states, controls, measurements, parameters, P, and K.
        ExtendedKalmanFilterHistory::AppendHistory(_appendTime);
        
        // And now the number of iterations.
        m_iekfIterationHistory.push_back( m_ptr_filter->GetNumIterations() );
        
        return;
}




/*! \brief Returns a matrix of the filter's iteration history.
 * @return This function returns a Vector of iteration counts.
 */
Vector IteratedExtendedKalmanFilterHistory::GetIterationHistory()
{
        Vector returnVector;
        Vector rightSize(m_TimeHistory.size());
        
        returnVector.initialize(rightSize); 
        
        for(unsigned int ii = 1; ii < m_TimeHistory.size(); ii++ )
        {
                returnVector(MatrixIndexBase + ii) = m_iekfIterationHistory[ii];
        }
        
        return returnVector;
}







// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: IteratedExtendedKalmanFilterHistory.cpp,v $
*       Revision 1.3  2003/11/01 21:00:14  rsharo
*       changed "Time.h" includes to "utils/Time.h" includes. Also eliminated excess compile warnings.
*       
*       Revision 1.2  2003/07/12 19:01:23  simpliciter
*       Fixed runtime error.
*       
*       Revision 1.1  2003/07/12 17:55:51  simpliciter
*       Initial submission.
*       
*       
*
******************************************************************************/

---