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 #include "iteratedextendedkalmanfilter.h"
00021
00022 namespace BFL
00023 {
00024 using namespace MatrixWrapper;
00025
00026
00027 #define AnalyticSys AnalyticSystemModelGaussianUncertainty
00028 #define AnalyticMeas AnalyticMeasurementModelGaussianUncertainty
00029
00030
00031 IteratedExtendedKalmanFilter::IteratedExtendedKalmanFilter(Gaussian* prior, unsigned int nr_it, InnovationCheck* I)
00032 : KalmanFilter(prior)
00033 , _nr_iterations(nr_it)
00034 , _innovationChecker(I)
00035 , _x(prior->DimensionGet())
00036 , _x_i(prior->DimensionGet())
00037 , _x_i_prev(prior->DimensionGet())
00038 , _J(prior->DimensionGet())
00039 , _innovation(prior->DimensionGet())
00040 , _F(prior->DimensionGet(),prior->DimensionGet())
00041 , _Q(prior->DimensionGet())
00042 , _P_Matrix(prior->DimensionGet())
00043 {}
00044
00045 IteratedExtendedKalmanFilter::~IteratedExtendedKalmanFilter()
00046 {}
00047
00048 void
00049 IteratedExtendedKalmanFilter::AllocateMeasModelIExt(const vector<unsigned int>& meas_dimensions)
00050 {
00051 unsigned int meas_dimension;
00052 for(int i = 0 ; i< meas_dimensions.size(); i++)
00053 {
00054
00055 meas_dimension = meas_dimensions[i];
00056 _mapMeasUpdateVariablesIExt_it = _mapMeasUpdateVariablesIExt.find(meas_dimension);
00057 if( _mapMeasUpdateVariablesIExt_it == _mapMeasUpdateVariablesIExt.end())
00058 {
00059
00060 _mapMeasUpdateVariablesIExt_it = (_mapMeasUpdateVariablesIExt.insert
00061 (std::pair<unsigned int, MeasUpdateVariablesIExt>( meas_dimension,MeasUpdateVariablesIExt(meas_dimension,_x.rows()) ))).first;
00062 }
00063 }
00064 }
00065
00066 void
00067 IteratedExtendedKalmanFilter::AllocateMeasModelIExt(const unsigned int& meas_dimension)
00068 {
00069
00070 _mapMeasUpdateVariablesIExt_it = _mapMeasUpdateVariablesIExt.find(meas_dimension);
00071 if( _mapMeasUpdateVariablesIExt_it == _mapMeasUpdateVariablesIExt.end())
00072 {
00073
00074 _mapMeasUpdateVariablesIExt_it = (_mapMeasUpdateVariablesIExt.insert
00075 (std::pair<unsigned int, MeasUpdateVariablesIExt>( meas_dimension,MeasUpdateVariablesIExt(meas_dimension,_x.rows()) ))).first;
00076 }
00077 }
00078
00079 void
00080 IteratedExtendedKalmanFilter::SysUpdate(SystemModel<ColumnVector>* const sysmodel,
00081 const ColumnVector& u)
00082 {
00083 _x = _post->ExpectedValueGet();
00084 _J = ((AnalyticSys*)sysmodel)->PredictionGet(u,_x);
00085 _F = ((AnalyticSys*)sysmodel)->df_dxGet(u,_x);
00086 _Q = ((AnalyticSys*)sysmodel)->CovarianceGet(u,_x);
00087
00088
00089
00090
00091
00092
00093
00094
00095 CalculateSysUpdate(_J, _F, _Q);
00096 }
00097
00098 void
00099 IteratedExtendedKalmanFilter::MeasUpdate(MeasurementModel<ColumnVector,ColumnVector>* const measmodel,
00100 const ColumnVector& z,
00101 const ColumnVector& s)
00102 {
00103
00104 AllocateMeasModelIExt(z.rows());
00105
00106 _x = _post->ExpectedValueGet();
00107 _P_Matrix = _post->CovarianceGet();
00108 _x_i = _post->ExpectedValueGet();
00109
00110 bool test_innovation = true;
00111
00112 for (unsigned int i=0; i<_nr_iterations && test_innovation; i++)
00113 {
00114 _x_i_prev = _x_i;
00115 (_mapMeasUpdateVariablesIExt_it->second)._H_i = ((AnalyticMeas*)measmodel)->df_dxGet(s,_x_i);
00116 (_mapMeasUpdateVariablesIExt_it->second)._R_i = ((AnalyticMeas*)measmodel)->CovarianceGet(s,_x_i);
00117 _S_i = ( (_mapMeasUpdateVariablesIExt_it->second)._H_i * (Matrix)_P_Matrix * ((_mapMeasUpdateVariablesIExt_it->second)._H_i.transpose()) ) + (Matrix)((_mapMeasUpdateVariablesIExt_it->second)._R_i);
00118 (_mapMeasUpdateVariablesIExt_it->second)._K_i = (Matrix)_P_Matrix * ((_mapMeasUpdateVariablesIExt_it->second)._H_i.transpose()) * (_S_i.inverse());
00119 (_mapMeasUpdateVariablesIExt_it->second)._Z_i = ((AnalyticMeas*)measmodel)->PredictionGet(s,_x_i) + ( (_mapMeasUpdateVariablesIExt_it->second)._H_i * (_x - _x_i) );
00120 _x_i = _x + (_mapMeasUpdateVariablesIExt_it->second)._K_i * (z - (_mapMeasUpdateVariablesIExt_it->second)._Z_i);
00121 _innovation = (_x_i - _x_i_prev);
00122 if (_innovationChecker != NULL)
00123 test_innovation = _innovationChecker->check(_innovation) ;
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133 }
00134
00135 CalculateMeasUpdate(z, (_mapMeasUpdateVariablesIExt_it->second)._Z_i, (_mapMeasUpdateVariablesIExt_it->second)._H_i, (_mapMeasUpdateVariablesIExt_it->second)._R_i);
00136 }
00137
00138 }
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 Mon Feb 11 2019 03:45:12