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_HEIGHT_H
00030 #define HECTOR_POSE_ESTIMATION_HEIGHT_H
00031
00032 #include <hector_pose_estimation/measurement.h>
00033
00034 namespace hector_pose_estimation {
00035
00036 class HeightModel : public MeasurementModel_<HeightModel,1> {
00037 public:
00038 HeightModel();
00039 virtual ~HeightModel();
00040
00041 virtual SystemStatus getStatusFlags() { return STATE_POSITION_Z; }
00042
00043 virtual void getMeasurementNoise(NoiseVariance& R, const State&, bool init);
00044 virtual void getExpectedValue(MeasurementVector& y_pred, const State& state);
00045 virtual void getStateJacobian(MeasurementMatrix& C, const State& state, bool init);
00046
00047 void setElevation(double elevation) { elevation_ = elevation; }
00048 double getElevation() const { return elevation_; }
00049
00050 protected:
00051 double stddev_;
00052 double elevation_;
00053 };
00054
00055 extern template class Measurement_<HeightModel>;
00056
00057 class HeightBaroCommon
00058 {
00059 public:
00060 HeightBaroCommon(Measurement *measurement);
00061 virtual ~HeightBaroCommon();
00062
00063 virtual void onReset();
00064 double resetElevation(const State &state, boost::function<double()> altitude_func);
00065
00066 private:
00067 bool auto_elevation_;
00068 bool elevation_initialized_;
00069 };
00070
00071 class Height : public Measurement_<HeightModel>, HeightBaroCommon
00072 {
00073 public:
00074 Height(const std::string& name = "height") : Measurement_<HeightModel>(name), HeightBaroCommon(this) {}
00075 virtual ~Height() {}
00076
00077 void setElevation(double elevation) { getModel()->setElevation(elevation); }
00078 double getElevation() const { return getModel()->getElevation(); }
00079
00080 virtual void onReset();
00081 virtual bool prepareUpdate(State &state, const Update &update);
00082 };
00083
00084 }
00085
00086 #endif // HECTOR_POSE_ESTIMATION_HEIGHT_H