29 #ifndef HECTOR_POSE_ESTIMATION_BARO_H 30 #define HECTOR_POSE_ESTIMATION_BARO_H 35 #ifdef USE_HECTOR_UAV_MSGS 36 #include <hector_uav_msgs/Altimeter.h> 91 Baro(
const std::string& name =
"baro");
94 void setElevation(
double elevation) { getModel()->setElevation(elevation); }
97 void setQnh(
double qnh) { getModel()->setQnh(qnh); }
98 double getQnh()
const {
return getModel()->getQnh(); }
100 virtual void onReset();
106 #endif // HECTOR_POSE_ESTIMATION_BARO_H virtual void getExpectedValue(MeasurementVector &y_pred, const State &state)
double getElevation() const
void setElevation(double elevation)
double getAltitude(const BaroUpdate &update)
virtual void getStateJacobian(MeasurementMatrix &C, const State &state, bool init)
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
virtual bool prepareUpdate(State &state, const MeasurementUpdate &update)
BaroUpdate & qnh(double qnh)
virtual bool init(PoseEstimation &estimator, Measurement &measurement, State &state)