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 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00040
00041 MagneticModel();
00042 virtual ~MagneticModel();
00043
00044 virtual bool init(PoseEstimation &estimator, Measurement &measurement, State &state);
00045
00046 virtual SystemStatus getStatusFlags() { return STATE_YAW; }
00047
00048 virtual void getMeasurementNoise(NoiseVariance& R, const State&, bool init);
00049 virtual void getExpectedValue(MeasurementVector& y_pred, const State& state);
00050 virtual void getStateJacobian(MeasurementMatrix& C, const State& state, bool init);
00051
00052 double getMagneticHeading(const State& state, const MeasurementVector& y) const;
00053 double getTrueHeading(const State& state, const MeasurementVector& y) const;
00054
00055 void setReference(const GlobalReference::Heading &reference_heading);
00056 bool hasMagnitude() const { return magnitude_ != 0.0; }
00057
00058 protected:
00059 double stddev_;
00060 double declination_, inclination_, magnitude_;
00061 void updateMagneticField();
00062
00063 MeasurementVector magnetic_field_north_;
00064 MeasurementVector magnetic_field_reference_;
00065 };
00066
00067 extern template class Measurement_<MagneticModel>;
00068
00069 class Magnetic : public Measurement_<MagneticModel>
00070 {
00071 public:
00072 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00073
00074 using Measurement_<MagneticModel>::Model;
00075 using Measurement_<MagneticModel>::Update;
00076
00077 enum { MeasurementDimension = Measurement_<MagneticModel>::MeasurementDimension };
00078 using Measurement_<MagneticModel>::MeasurementVector;
00079 using Measurement_<MagneticModel>::NoiseVariance;
00080
00081 Magnetic(const std::string& name = "height");
00082 virtual ~Magnetic() {}
00083
00084 virtual void onReset();
00085
00086 virtual MeasurementVector const& getVector(const Update &update, const State&);
00087
00088
00089 virtual bool prepareUpdate(State &state, const Update &update);
00090
00091 private:
00092 bool auto_heading_;
00093 GlobalReferencePtr reference_;
00094 ColumnVector deviation_;
00095
00096 MeasurementVector y_;
00097 NoiseVariance R_;
00098 };
00099
00100 }
00101
00102 #endif // HECTOR_POSE_ESTIMATION_MAGNETIC_H