Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #ifndef __ITERATED_EXTENDED_KALMAN_FILTER__
00022 #define __ITERATED_EXTENDED_KALMAN_FILTER__
00023
00024 #include "kalmanfilter.h"
00025 #include "../pdf/conditionalpdf.h"
00026 #include "../pdf/gaussian.h"
00027 #include "innovationCheck.h"
00028 # include <map>
00029
00030 namespace BFL
00031 {
00040 class IteratedExtendedKalmanFilter : public KalmanFilter
00041 {
00042 protected:
00043 virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
00044 const MatrixWrapper::ColumnVector& u);
00045 virtual void MeasUpdate(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,
00046 const MatrixWrapper::ColumnVector& z,
00047 const MatrixWrapper::ColumnVector& s);
00048
00049 public:
00056 IteratedExtendedKalmanFilter(Gaussian* prior, unsigned int nr_it=1, InnovationCheck* innov = NULL);
00057
00059 virtual ~IteratedExtendedKalmanFilter();
00060
00062
00063
00064
00065
00066 void AllocateMeasModelIExt( const vector<unsigned int>& meas_dimensions);
00067
00069
00070
00071
00072
00073 void AllocateMeasModelIExt( const unsigned int& meas_dimensions);
00074
00075 private:
00077 unsigned int _nr_iterations;
00079 InnovationCheck* _innovationChecker;
00080
00081 struct MeasUpdateVariablesIExt
00082 {
00083 SymmetricMatrix _R_i;
00084 Matrix _K_i;
00085 Matrix _H_i;
00086 ColumnVector _Z_i;
00087 MeasUpdateVariablesIExt() {};
00088 MeasUpdateVariablesIExt(unsigned int meas_dimension, unsigned int state_dimension):
00089 _R_i(meas_dimension)
00090 , _K_i(state_dimension,meas_dimension)
00091 , _H_i(meas_dimension,state_dimension)
00092 , _Z_i(meas_dimension)
00093 {};
00094 };
00095
00096
00097 ColumnVector _x;
00098 ColumnVector _x_i;
00099 ColumnVector _x_i_prev;
00100 ColumnVector _J;
00101 ColumnVector _innovation;
00102 Matrix _F;
00103 SymmetricMatrix _Q;
00104 SymmetricMatrix _P_Matrix;
00105 Matrix _S_i;
00106 std::map<unsigned int, MeasUpdateVariablesIExt> _mapMeasUpdateVariablesIExt;
00107 std::map<unsigned int, MeasUpdateVariablesIExt>::iterator _mapMeasUpdateVariablesIExt_it;
00108 };
00109
00110 }
00111
00112 #endif // __ITERATED_EXTENDED_KALMAN_FILTER__
00113
bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Sun Oct 5 2014 22:29:52