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 #include <hector_pose_estimation/measurements/baro.h>
00030 #include <hector_pose_estimation/filter/set_filter.h>
00031
00032 #include <boost/bind.hpp>
00033
00034 namespace hector_pose_estimation {
00035
00036 template class Measurement_<BaroModel>;
00037
00038 BaroModel::BaroModel()
00039 {
00040 stddev_ = 1.0;
00041 qnh_ = 1013.25;
00042 parameters().add("qnh", qnh_);
00043 }
00044
00045 BaroModel::~BaroModel() {}
00046
00047 void BaroModel::getExpectedValue(MeasurementVector& y_pred, const State& state)
00048 {
00049 y_pred(0) = qnh_ * pow(1.0 - (0.0065 * (state.getPosition().z() + getElevation())) / 288.15, 5.255);
00050 }
00051
00052 void BaroModel::getStateJacobian(MeasurementMatrix& C, const State& state, bool)
00053 {
00054 if (state.position()) {
00055 state.position()->cols(C)(0,Z) = qnh_ * 5.255 * pow(1.0 - (0.0065 * (state.getPosition().z() + getElevation())) / 288.15, 4.255) * (-0.0065 / 288.15);
00056 }
00057 }
00058
00059 double BaroModel::getAltitude(const BaroUpdate& update)
00060 {
00061 return 288.15 / 0.0065 * (1.0 - pow(update.getVector()(0) / qnh_, 1.0/5.255));
00062 }
00063
00064 BaroUpdate::BaroUpdate() : qnh_(0) {}
00065 BaroUpdate::BaroUpdate(double pressure) : qnh_(0) { *this = pressure; }
00066 BaroUpdate::BaroUpdate(double pressure, double qnh) : qnh_(qnh) { *this = pressure; }
00067
00068 Baro::Baro(const std::string &name)
00069 : Measurement_<BaroModel>(name)
00070 , HeightBaroCommon(this)
00071 {
00072 parameters().add("auto_elevation", auto_elevation_);
00073 }
00074
00075 void Baro::onReset()
00076 {
00077 HeightBaroCommon::onReset();
00078 }
00079
00080 bool Baro::prepareUpdate(State &state, const Update &update) {
00081 if (update.qnh() != 0) setQnh(update.qnh());
00082
00083 setElevation(resetElevation(state, boost::bind(&BaroModel::getAltitude, getModel(), update)));
00084 return true;
00085 }
00086
00087 }