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 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00041
00042 EKF(State &state);
00043 virtual ~EKF();
00044
00045 virtual std::string getType() const { return "EKF"; }
00046
00047 virtual bool init(PoseEstimation &estimator);
00048 virtual bool preparePredict(double dt);
00049 virtual bool predict(const SystemPtr& system, double dt);
00050 virtual bool doPredict(double dt);
00051
00052 class Predictor
00053 {
00054 public:
00055 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00056
00057 Predictor(EKF *filter)
00058 : filter_(filter)
00059 , x_diff(filter->state().getVectorDimension())
00060 , A(filter->state().getCovarianceDimension(),filter->state().getCovarianceDimension())
00061 , Q(filter->state().getCovarianceDimension())
00062 {}
00063 virtual ~Predictor() {}
00064 virtual bool predict(double dt) = 0;
00065
00066 protected:
00067 EKF *filter_;
00068
00069 public:
00070 State::Vector x_diff;
00071 State::SystemMatrix A;
00072 State::Covariance Q;
00073 };
00074
00075 template <class ConcreteModel, typename Enabled = void>
00076 class Predictor_ : public Filter::template Predictor_<ConcreteModel>, public Predictor
00077 {
00078 public:
00079 typedef ConcreteModel Model;
00080 typedef typename Filter::template Predictor_<ConcreteModel> Base;
00081 using Filter::template Predictor_<ConcreteModel>::state;
00082
00083 Predictor_(EKF *filter, Model *model)
00084 : Base(filter, model)
00085 , Predictor(filter)
00086 {}
00087 virtual ~Predictor_() {}
00088
00089 virtual bool predict(double dt);
00090 };
00091
00092 class Corrector
00093 {
00094 public:
00095 Corrector(EKF *filter) : filter_(filter) {}
00096 virtual ~Corrector() {}
00097
00098 protected:
00099 EKF *filter_;
00100 };
00101
00102 template <class ConcreteModel, typename Enabled = void>
00103 class Corrector_ : public Filter::template Corrector_<ConcreteModel>, public Corrector
00104 {
00105 public:
00106 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00107
00108 typedef ConcreteModel Model;
00109 typedef typename Filter::template Corrector_<ConcreteModel> Base;
00110 using Filter::template Corrector_<ConcreteModel>::state;
00111
00112 Corrector_(EKF *filter, Model *model)
00113 : Base(filter, model)
00114 , Corrector(filter)
00115 , y_pred(model->getDimension())
00116 , error(model->getDimension())
00117 , C(model->getDimension(), filter->state().getCovarianceDimension())
00118 , CP(model->getDimension(), filter->state().getCovarianceDimension())
00119 , S(model->getDimension())
00120 , K(filter->state().getCovarianceDimension(), model->getDimension())
00121 , update(filter->state().getCovarianceDimension())
00122 {}
00123 virtual ~Corrector_() {}
00124
00125 virtual bool correct(const typename ConcreteModel::MeasurementVector& y, const typename ConcreteModel::NoiseVariance& R);
00126 virtual typename ConcreteModel::MeasurementVector getResidual() const { return error; }
00127
00128 public:
00129 typename Model::MeasurementVector y_pred;
00130 typename Model::MeasurementVector error;
00131 typename Model::MeasurementMatrix C;
00132 Matrix_<ConcreteModel::MeasurementDimension, Dynamic> CP;
00133 typename Model::NoiseVariance S;
00134 typename Model::GainMatrix K;
00135 typename Model::UpdateVector update;
00136 };
00137
00138 public:
00139 State::Vector x_diff;
00140 State::SystemMatrix A;
00141 State::Covariance Q;
00142 };
00143
00144 }
00145 }
00146
00147 #include "ekf.inl"
00148
00149 #endif // HECTOR_POSE_ESTIMATION_FILTER_EKF_H