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/height.h>
00030 #include <hector_pose_estimation/pose_estimation.h>
00031 #include <hector_pose_estimation/global_reference.h>
00032 #include <hector_pose_estimation/filter/set_filter.h>
00033
00034 #include <ros/console.h>
00035
00036 namespace hector_pose_estimation {
00037
00038 template class Measurement_<HeightModel>;
00039
00040 HeightModel::HeightModel()
00041 {
00042 stddev_ = 10.0;
00043 elevation_ = 0.0;
00044 parameters().add("stddev", stddev_);
00045 }
00046
00047 HeightModel::~HeightModel() {}
00048
00049 void HeightModel::getMeasurementNoise(NoiseVariance& R, const State&, bool init)
00050 {
00051 if (init) {
00052 R(0,0) = pow(stddev_, 2);
00053 }
00054 }
00055
00056 void HeightModel::getExpectedValue(MeasurementVector& y_pred, const State& state)
00057 {
00058 y_pred(0) = state.getPosition().z() + getElevation();
00059 }
00060
00061 void HeightModel::getStateJacobian(MeasurementMatrix& C, const State& state, bool init)
00062 {
00063 if (!init) return;
00064
00065 if (state.position()) {
00066 state.position()->cols(C)(0,Z) = 1.0;
00067 }
00068 }
00069
00070 HeightBaroCommon::HeightBaroCommon(Measurement* parent)
00071 : auto_elevation_(true)
00072 , elevation_initialized_(false)
00073 {
00074 }
00075
00076 HeightBaroCommon::~HeightBaroCommon() {}
00077
00078 void HeightBaroCommon::onReset() {
00079 elevation_initialized_ = false;
00080 }
00081
00082 double HeightBaroCommon::resetElevation(const State &state, boost::function<double()> altitude_func) {
00083 if (!elevation_initialized_) {
00084 if (auto_elevation_) GlobalReference::Instance()->setCurrentAltitude(state, altitude_func());
00085 elevation_initialized_ = true;
00086 }
00087
00088 return GlobalReference::Instance()->position().altitude;
00089 }
00090
00091 Height::Height(const std::string &name)
00092 : Measurement_<HeightModel>(name)
00093 , HeightBaroCommon(this)
00094 {
00095 parameters().add("auto_elevation", auto_elevation_);
00096 }
00097
00098 void Height::onReset() {
00099 HeightBaroCommon::onReset();
00100 }
00101
00102 template <typename T> struct functor_wrapper
00103 {
00104 functor_wrapper(const T& value) : value(value) {}
00105 T& operator()() { return value; }
00106 const T& operator()() const { return value; }
00107 private:
00108 T value;
00109 };
00110
00111 bool Height::prepareUpdate(State &state, const Update &update) {
00112 setElevation(resetElevation(state, functor_wrapper<double>(update.getVector()(0))));
00113 return true;
00114 }
00115
00116 }