KalmanFilterHistory.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file       KalmanFilterHistory.cpp
 *  \brief      A class to store additional data specific to the Kalman Filter.  Derived from SequentialFilter.
 *  \author $Author: rsharo $
 *  \version $Revision: 1.3 $
 *  \date    $Date: 2003/11/01 21:00:15 $
 *//////////////////////////////////////////////////////////////////////////////////////////////////
/*  
 */
//////////////////////////////////////////////////////////////////////////////////////////////////

#include "KalmanFilterHistory.h"

/*! \brief Only one constructor.  (No empty constructor.)
 * Builds an empty history for the filter instance passed in as a pointer.  */
KalmanFilterHistory::KalmanFilterHistory(SequentialFilter* _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);
        
        // 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 KalmanFilterHistory::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 KalmanFilterHistory::AppendHistory(const ssfTime& _appendTime)
{
        // This guy logs the time, states, controls, measurements, parameters.
        SequentialFilterHistory::AppendHistory(_appendTime);
        
        // And now the P and K matrices.
        m_kfCovarianceHistory.push_back( m_ptr_filter->GetCovarianceMatrix() );
        m_kfKalmanGainHistory.push_back( m_ptr_filter->GetKalmanGainMatrix() );
        
        return;
}



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



/*! \brief Returns a matrix of the filter's basic history (no Kalman information).
 * @return This function returns a matrix in the form [t_k, state(t_k), control(t_k), measurement(t_k), parameter(t_k)]
 */
Matrix KalmanFilterHistory::GetHistory()
{
        Matrix returnMatrix;
        
        //Let SequentialFilterHistory do the work.
        returnMatrix = SequentialFilterHistory::GetHistory();
        
        return returnMatrix;
}



/*! Returns basic and Kalman history data by reference.
 *
 * @return This function returns a void but passes the default history and additional 
 * Kalman Filter history information by reference in two separate matrices.
 *
 * The first matrix has (num time steps) rows 
 * and is of the form [t_k, state(t_k), control(t_k), measurement(t_k), parameter(t_k)].
 * The second matrix has (num time steps)*(num states) rows and (num states + num measurements) columns;
 * data is stored as [ P(t_k), K(t_k) ].  Note that each matrix takes up (num states) rows.
 */
void KalmanFilterHistory::GetKalmanHistory(Matrix& _sfHistMatrix, Matrix& _kfHistMatrix)
{
        // sequential filter data
        _sfHistMatrix = SequentialFilterHistory::GetHistory();
        
        // kalman filter data
        // note that the covariance matrix is nxn and the kalman gain matrix is nxm
        int StateVectorSize = m_ptr_filter->GetStateVector().getIndexBound();                           // length n
        int MeasurementVectorSize = m_ptr_filter->GetMeasurementVector().getIndexBound();       // length m
                
    Matrix rightSize(m_TimeHistory.size()*StateVectorSize, StateVectorSize + MeasurementVectorSize);
        
        _kfHistMatrix.initialize(rightSize); 
        
        for(unsigned int ii = 0; ii*StateVectorSize < m_TimeHistory.size()*StateVectorSize; ii++ )
        {
                _kfHistMatrix( _( MatrixIndexBase + ii*StateVectorSize, MatrixIndexBase + ii*StateVectorSize + StateVectorSize - 1 ),
                                   _( MatrixIndexBase, MatrixIndexBase + StateVectorSize - 1 ) ) = m_kfCovarianceHistory[ii];
                _kfHistMatrix( _( MatrixIndexBase + ii*StateVectorSize, MatrixIndexBase + ii*StateVectorSize + StateVectorSize - 1 ),
                                   _( MatrixIndexBase + StateVectorSize , MatrixIndexBase + StateVectorSize + MeasurementVectorSize - 1 ) ) = m_kfKalmanGainHistory[ii];
        }
}




// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: KalmanFilterHistory.cpp,v $
*       Revision 1.3  2003/11/01 21:00:15  rsharo
*       changed "Time.h" includes to "utils/Time.h" includes. Also eliminated excess compile warnings.
*       
*       Revision 1.2  2003/06/28 00:46:06  simpliciter
*       Finished GetKalmanHistory.  Huzzah!
*       
*       Revision 1.1  2003/06/27 01:52:50  simpliciter
*       Fully commented!  And almost fully functional.
*       Once I learn the best way to copy chunks of one Matrix
*       into another I'll finish GetKalmanHistory.  Otherwise
*       everything is done.  GetState is not implemented because
*       it would require / desire interpolations, which seems out
*       of spirit with tracking the effectiveness 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.
*       
*
******************************************************************************/

---