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
00022
00023
00024
00025
00026
00027
00028
00029 #ifndef HECTOR_POSE_ESTIMATION_FILTER_EKF_H
00030 #define HECTOR_POSE_ESTIMATION_FILTER_EKF_H
00031
00032 #include <hector_pose_estimation/filter.h>
00033
00034 namespace hector_pose_estimation {
00035 namespace filter {
00036
00037 class EKF : public Filter
00038 {
00039 public:
00040 EKF();
00041 virtual ~EKF();
00042
00043 virtual std::string getType() const { return "EKF"; }
00044
00045 virtual bool init(PoseEstimation &estimator);
00046 virtual bool doPredict(double dt);
00047
00048 template <class ConcreteModel, typename Enabled = void>
00049 class PredictorImpl_ : public Filter::template Predictor_<ConcreteModel>
00050 {
00051 public:
00052 typedef ConcreteModel Model;
00053 typedef typename Filter::template Predictor_<ConcreteModel> Base;
00054 using Filter::template Predictor_<ConcreteModel>::state;
00055
00056 PredictorImpl_(EKF *filter, Model *model)
00057 : Base(filter, model)
00058 , filter_(filter)
00059 , x_pred(filter_->x_pred.segment<State::Dimension>(0))
00060 , A(filter_->A.block<State::Dimension,State::Dimension>(0,0))
00061 , Q(filter->Q.block<State::Dimension,State::Dimension>(0,0))
00062 {}
00063 virtual ~PredictorImpl_() {}
00064
00065 virtual bool predict(double dt);
00066
00067 protected:
00068 EKF *filter_;
00069
00070 public:
00071 typename Model::StateVectorSegment x_pred;
00072 typename Model::SystemMatrixBlock A;
00073 typename Model::NoiseVarianceBlock Q;
00074 };
00075
00076 template <class ConcreteModel>
00077 class PredictorImpl_<ConcreteModel, typename boost::enable_if<class ConcreteModel::IsSubSystem >::type> : public Filter::template Predictor_<ConcreteModel>
00078 {
00079 public:
00080 typedef ConcreteModel Model;
00081 typedef typename Filter::template Predictor_<ConcreteModel> Base;
00082 using Filter::template Predictor_<ConcreteModel>::state;
00083 using Filter::template Predictor_<ConcreteModel>::sub;
00084
00085 PredictorImpl_(EKF *filter, Model *model)
00086 : Base(filter, model)
00087 , filter_(filter)
00088 , x_pred(filter_->x_pred.segment<ConcreteModel::Dimension>(model->getStateIndex(state())))
00089 , A11(filter_->A.block<ConcreteModel::Dimension,ConcreteModel::Dimension>(model->getStateIndex(state()), model->getStateIndex(state())))
00090 , A01(filter_->A.block<State::Dimension,ConcreteModel::Dimension>(0, model->getStateIndex(state())))
00091 , Q1(filter->Q.block<ConcreteModel::Dimension,ConcreteModel::Dimension>(model->getStateIndex(state()), model->getStateIndex(state())))
00092 {}
00093 virtual ~PredictorImpl_() {}
00094
00095 virtual bool predict(double dt);
00096
00097 protected:
00098 EKF *filter_;
00099
00100 public:
00101 typename Model::StateVectorSegment x_pred;
00102 typename Model::SystemMatrixBlock A11;
00103 typename Model::CrossSystemMatrixBlock A01;
00104 typename Model::NoiseVarianceBlock Q1;
00105 };
00106
00107 template <class ConcreteModel, typename Enabled = void>
00108 class CorrectorImpl_ : public Filter::template Corrector_<ConcreteModel>
00109 {
00110 public:
00111 typedef ConcreteModel Model;
00112 typedef typename Filter::template Corrector_<ConcreteModel> Base;
00113 using Filter::template Corrector_<ConcreteModel>::state;
00114
00115 CorrectorImpl_(EKF *filter, Model *model) : Base(filter, model), filter_(filter) {}
00116 virtual ~CorrectorImpl_() {}
00117
00118 virtual bool correct(const typename ConcreteModel::MeasurementVector& y, const typename ConcreteModel::NoiseVariance& R);
00119
00120 protected:
00121 EKF *filter_;
00122
00123 public:
00124 typename Model::MeasurementVector y_pred;
00125 typename Model::MeasurementVector error;
00126 typename Model::MeasurementMatrix C;
00127 Matrix_<ConcreteModel::MeasurementDimension, Dynamic> CP;
00128 typename Model::NoiseVariance S;
00129 typename Model::GainMatrix K;
00130 };
00131
00132 template <class ConcreteModel>
00133 class CorrectorImpl_<ConcreteModel, typename boost::enable_if<class ConcreteModel::HasSubSystem >::type> : public Filter::template Corrector_<ConcreteModel>
00134 {
00135 public:
00136 typedef ConcreteModel Model;
00137 typedef typename Filter::template Corrector_<ConcreteModel> Base;
00138 using Filter::template Corrector_<ConcreteModel>::state;
00139 using Filter::template Corrector_<ConcreteModel>::sub;
00140
00141 CorrectorImpl_(EKF *filter, Model *model) : Base(filter, model), filter_(filter) {}
00142 virtual ~CorrectorImpl_() {}
00143
00144 virtual bool correct(const typename ConcreteModel::MeasurementVector& y, const typename ConcreteModel::NoiseVariance& R);
00145
00146 protected:
00147 EKF *filter_;
00148
00149 public:
00150 typename Model::MeasurementVector y_pred;
00151 typename Model::MeasurementVector error;
00152 typename Model::MeasurementMatrix C0;
00153 typename Model::SubMeasurementMatrix C1;
00154 Matrix_<ConcreteModel::MeasurementDimension, Dynamic> CP;
00155 typename Model::NoiseVariance S;
00156 typename Model::GainMatrix K;
00157 };
00158
00159 template <class ConcreteModel>
00160 class Predictor_ : public PredictorImpl_<ConcreteModel> {
00161 public:
00162 Predictor_(EKF *filter, ConcreteModel *model) : PredictorImpl_<ConcreteModel>(filter, model) {}
00163 virtual ~Predictor_() {}
00164 };
00165
00166 template <class ConcreteModel>
00167 class Corrector_ : public CorrectorImpl_<ConcreteModel> {
00168 public:
00169 Corrector_(EKF *filter, ConcreteModel *model) : CorrectorImpl_<ConcreteModel>(filter, model) {}
00170 virtual ~Corrector_() {}
00171 };
00172
00173 public:
00174 State::Vector x_pred;
00175 Matrix A;
00176 SymmetricMatrix Q;
00177 };
00178
00179 }
00180 }
00181
00182 #include "ekf.inl"
00183
00184 #endif // HECTOR_POSE_ESTIMATION_FILTER_EKF_H