21 #ifndef __ITERATED_EXTENDED_KALMAN_FILTER__
22 #define __ITERATED_EXTENDED_KALMAN_FILTER__
24 #include "kalmanfilter.h"
25 #include "../pdf/conditionalpdf.h"
26 #include "../pdf/gaussian.h"
27 #include "innovationCheck.h"
44 const MatrixWrapper::ColumnVector& u);
46 const MatrixWrapper::ColumnVector& z,
47 const MatrixWrapper::ColumnVector& s);
77 unsigned int _nr_iterations;
81 struct MeasUpdateVariablesIExt
87 MeasUpdateVariablesIExt() {};
88 MeasUpdateVariablesIExt(
unsigned int meas_dimension,
unsigned int state_dimension):
90 , _K_i(state_dimension,meas_dimension)
91 , _H_i(meas_dimension,state_dimension)
92 , _Z_i(meas_dimension)
99 ColumnVector _x_i_prev;
101 ColumnVector _innovation;
104 SymmetricMatrix _P_Matrix;
106 std::map<unsigned int, MeasUpdateVariablesIExt> _mapMeasUpdateVariablesIExt;
107 std::map<unsigned int, MeasUpdateVariablesIExt>::iterator _mapMeasUpdateVariablesIExt_it;
Class representing Gaussian (or normal density)
Class implementing an innovationCheck used in IEKF.
virtual void MeasUpdate(MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)
Measurement Update (overloaded)
void AllocateMeasModelIExt(const unsigned int &meas_dimensions)
Function to allocate memory needed during the measurement update.
IteratedExtendedKalmanFilter(Gaussian *prior, unsigned int nr_it=1, InnovationCheck *innov=NULL)
virtual void SysUpdate(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u)
System Update.
void AllocateMeasModelIExt(const vector< unsigned int > &meas_dimensions)
Function to allocate memory needed during the measurement update,.
virtual ~IteratedExtendedKalmanFilter()
Destructor.
Class representing the family of all Kalman Filters (EKF, IEKF, ...)