ekf.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2013, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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 } // namespace filter
00180 } // namespace hector_pose_estimation
00181 
00182 #include "ekf.inl"
00183 
00184 #endif // HECTOR_POSE_ESTIMATION_FILTER_EKF_H


hector_pose_estimation_core
Author(s): Johannes Meyer
autogenerated on Mon Oct 6 2014 00:24:16