SequentialFilterHistory.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file       SequentialFilterHistory.cpp
 *  \brief      An abstract class to store generic sequential filter data.
 *  \author $Author: rsharo $
 *  \version $Revision: 1.4 $
 *  \date    $Date: 2003/11/01 21:00:15 $
 *//////////////////////////////////////////////////////////////////////////////////////////////////
/*  
 */
//////////////////////////////////////////////////////////////////////////////////////////////////

#include "SequentialFilterHistory.h"
    

/*! @brief Protected initializer function.  A pseudo-constuctor for this abstract class, if you will.
 * Takes a pointer to the filter being logged; called from dervied class constructor. */
void SequentialFilterHistory::InitializeSequentialFilterHistory(SequentialFilter* _ptr_filter)
{
        // clean start
        ResetHistory();
        
        // store that protected data member!
        m_ptr_filter = _ptr_filter;
        
        // allocate space for the vector containers
        m_sfStateHistory.reserve(HISTORY_RESERVE_SIZE);
        m_sfControlHistory.reserve(HISTORY_RESERVE_SIZE);
        m_sfMeasurementHistory.reserve(HISTORY_RESERVE_SIZE);
        m_sfParameterHistory.reserve(HISTORY_RESERVE_SIZE);
};



/*! \brief Add the sequential 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.
 *
 */
void SequentialFilterHistory::AppendHistory(const double& _appendTime)
{
    AppendHistory(ssfTime(_appendTime));
}    
    


/*! \brief Add the sequential 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.
 * 
 */
void SequentialFilterHistory::AppendHistory(const ssfTime& _appendTime)
{
        // time stuff
        History::AppendHistory(_appendTime);
        
        // the four vectors
        m_sfStateHistory.push_back( m_ptr_filter->GetStateVector() );
        m_sfControlHistory.push_back( m_ptr_filter->GetControlVector() );
        m_sfMeasurementHistory.push_back( m_ptr_filter->GetMeasurementVector() );
        m_sfParameterHistory.push_back( m_ptr_filter->GetParameterVector() );
        
        return;
}



/*! \brief Erases the sequential filters's struct history */
void SequentialFilterHistory::ResetHistory() { }



/*! \brief Returns a matrix of the sequential filter's struct history
 * @return This function returns a matrix in the form [t_k, state(t_k), control(t_k), measurement(t_k), parameter(t_k)]
 */
Matrix SequentialFilterHistory::GetHistory()
{
        // initialize the output matrix based on the size of the desired output state, 
        // and an element for time. The number of rows is equal to the number of time elements.
        int StateVectorSize = m_ptr_filter->GetStateVector().getIndexBound();
        int ControlVectorSize = m_ptr_filter->GetControlVector().getIndexBound();
        int MeasurementVectorSize = m_ptr_filter->GetMeasurementVector().getIndexBound();
        int ParameterVectorSize = m_ptr_filter->GetParameterVector().getIndexBound();
        
        Matrix returnMatrix(m_TimeHistory.size(), 1 + StateVectorSize + ControlVectorSize + MeasurementVectorSize + ParameterVectorSize);
    
        // and stick it in the matrx!   
        for(unsigned int ii = 0; ii < m_TimeHistory.size(); ii++)
                {
                returnMatrix(MatrixIndexBase + ii, MatrixIndexBase) = m_TimeHistory[ii].GetSeconds();
                returnMatrix(MatrixIndexBase + ii, _(MatrixIndexBase + 1, MatrixIndexBase + StateVectorSize)) = ~m_sfStateHistory[ii];
                returnMatrix(MatrixIndexBase + ii, _(MatrixIndexBase + 1 + StateVectorSize, MatrixIndexBase + StateVectorSize + ControlVectorSize)) = ~m_sfControlHistory[ii];
                returnMatrix(MatrixIndexBase + ii, _(MatrixIndexBase + 1 + StateVectorSize + ControlVectorSize, MatrixIndexBase + StateVectorSize + ControlVectorSize + MeasurementVectorSize)) = ~m_sfMeasurementHistory[ii];
                returnMatrix(MatrixIndexBase + ii, _(MatrixIndexBase + 1 + StateVectorSize + ControlVectorSize + MeasurementVectorSize, MatrixIndexBase + StateVectorSize + ControlVectorSize + MeasurementVectorSize + ParameterVectorSize)) = ~m_sfParameterHistory[ii];
                }
        
        return returnMatrix;
}


// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: SequentialFilterHistory.cpp,v $
*       Revision 1.4  2003/11/01 21:00:15  rsharo
*       changed "Time.h" includes to "utils/Time.h" includes. Also eliminated excess compile warnings.
*       
*       Revision 1.3  2003/06/28 00:45:37  simpliciter
*       Basic housekeeping.
*       
*       Revision 1.2  2003/06/27 01:53:56  simpliciter
*       Fully commented and fully functional!
*       GetState not implemented because it would require / desire
*       iterpolations, which seems not in the spirit of logging the
*       performance of a filter.
*       
*       Revision 1.1  2003/06/24 15:50:06  simpliciter
*       Everything but the all important "GetHistory" command
*       should work.  :)
*       
*       Revision 1.0  2003/06/06 18:44:15  simpliciter
*       Initial submission.
*       
*
******************************************************************************/

---