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(), filter->state().getCovarianceDimension())
00062 {
00063 x_diff.setZero();
00064 A.setZero();
00065 Q.setZero();
00066 }
00067 virtual ~Predictor() {}
00068 virtual bool predict(double dt) = 0;
00069
00070 protected:
00071 EKF *filter_;
00072
00073 public:
00074 State::Vector x_diff;
00075 State::SystemMatrix A;
00076 State::Covariance Q;
00077 };
00078
00079 template <class ConcreteModel, typename Enabled = void>
00080 class Predictor_ : public Filter::template Predictor_<ConcreteModel>, public Predictor
00081 {
00082 public:
00083 typedef ConcreteModel Model;
00084 typedef typename Filter::template Predictor_<ConcreteModel> Base;
00085 using Filter::template Predictor_<ConcreteModel>::state;
00086
00087 Predictor_(EKF *filter, Model *model)
00088 : Base(filter, model)
00089 , Predictor(filter)
00090 {}
00091 virtual ~Predictor_() {}
00092
00093 virtual bool predict(double dt);
00094 };
00095
00096 class Corrector
00097 {
00098 public:
00099 Corrector(EKF *filter) : filter_(filter) {}
00100 virtual ~Corrector() {}
00101
00102 protected:
00103 EKF *filter_;
00104 };
00105
00106 template <class ConcreteModel, typename Enabled = void>
00107 class Corrector_ : public Filter::template Corrector_<ConcreteModel>, public Corrector
00108 {
00109 public:
00110 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00111
00112 typedef ConcreteModel Model;
00113 typedef typename Filter::template Corrector_<ConcreteModel> Base;
00114 using Filter::template Corrector_<ConcreteModel>::state;
00115
00116 Corrector_(EKF *filter, Model *model)
00117 : Base(filter, model)
00118 , Corrector(filter)
00119 , y_pred(model->getDimension())
00120 , error(model->getDimension())
00121 , C(model->getDimension(), filter->state().getCovarianceDimension())
00122 , CP(model->getDimension(), filter->state().getCovarianceDimension())
00123 , S(model->getDimension(), model->getDimension())
00124 , K(filter->state().getCovarianceDimension(), model->getDimension())
00125 , update(filter->state().getCovarianceDimension())
00126 {
00127 y_pred.setZero();
00128 error.setZero();
00129 C.setZero();
00130 CP.setZero();
00131 S.setZero();
00132 K.setZero();
00133 update.setZero();
00134 }
00135 virtual ~Corrector_() {}
00136
00137 virtual bool correct(const typename ConcreteModel::MeasurementVector& y, const typename ConcreteModel::NoiseVariance& R);
00138 virtual typename ConcreteModel::MeasurementVector getResidual() const { return error; }
00139
00140 public:
00141 typename Model::MeasurementVector y_pred;
00142 typename Model::MeasurementVector error;
00143 typename Model::MeasurementMatrix C;
00144 typename Matrix_<ConcreteModel::MeasurementDimension, Dynamic>::type CP;
00145 typename Model::NoiseVariance S;
00146 typename Model::GainMatrix K;
00147 typename Model::UpdateVector update;
00148 };
00149
00150 public:
00151 State::Vector x_diff;
00152 State::SystemMatrix A;
00153 State::Covariance Q;
00154 };
00155
00156 }
00157 }
00158
00159 #include "ekf.inl"
00160
00161 #endif // HECTOR_POSE_ESTIMATION_FILTER_EKF_H