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_MAGNETIC_H
00030 #define HECTOR_POSE_ESTIMATION_MAGNETIC_H
00031
00032 #include <hector_pose_estimation/measurement.h>
00033 #include <hector_pose_estimation/global_reference.h>
00034
00035 namespace hector_pose_estimation {
00036
00037 class MagneticModel : public MeasurementModel_<MagneticModel,3> {
00038 public:
00039 MagneticModel();
00040 virtual ~MagneticModel();
00041
00042 virtual bool init(PoseEstimation &estimator, State &state);
00043
00044 virtual SystemStatus getStatusFlags() { return STATE_YAW; }
00045
00046 virtual void getMeasurementNoise(NoiseVariance& R, const State&, bool init);
00047 virtual void getExpectedValue(MeasurementVector& y_pred, const State& state);
00048 virtual void getStateJacobian(MeasurementMatrix& C, const State& state, bool init);
00049
00050 double getMagneticHeading(const State& state, const MeasurementVector& y) const;
00051 double getTrueHeading(const State& state, const MeasurementVector& y) const;
00052
00053 void setReference(const GlobalReference::Heading &reference_heading);
00054 bool hasMagnitude() const { return magnitude_ != 0.0; }
00055
00056 protected:
00057 double stddev_;
00058 double declination_, inclination_, magnitude_;
00059 void updateMagneticField();
00060
00061 MeasurementVector magnetic_field_north_;
00062 MeasurementVector magnetic_field_reference_;
00063
00064 };
00065
00066 extern template class Measurement_<MagneticModel>;
00067
00068 class Magnetic : public Measurement_<MagneticModel>
00069 {
00070 public:
00071 Magnetic(const std::string& name = "height");
00072 virtual ~Magnetic() {}
00073
00074 virtual void onReset();
00075
00076 virtual MeasurementVector const& getVector(const Update &update, const State&);
00077 virtual NoiseVariance const& getVariance(const Update &update, const State&);
00078
00079 virtual bool prepareUpdate(State &state, const Update &update);
00080
00081 private:
00082 bool auto_heading_;
00083 GlobalReferencePtr reference_;
00084 ColumnVector deviation_;
00085
00086 MeasurementVector y_;
00087 NoiseVariance R_;
00088 };
00089
00090 }
00091
00092 #endif // HECTOR_POSE_ESTIMATION_MAGNETIC_H