sensor_model.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef HECTOR_GAZEBO_PLUGINS_SENSOR_MODEL_H
00030 #define HECTOR_GAZEBO_PLUGINS_SENSOR_MODEL_H
00031 
00032 #include <sdf/sdf.hh>
00033 #include <gazebo/math/Vector3.hh>
00034 
00035 #include <hector_gazebo_plugins/SensorModelConfig.h>
00036 #include <numeric>
00037 
00038 namespace gazebo {
00039 
00040 using hector_gazebo_plugins::SensorModelConfig;
00041 
00042 template <typename T>
00043 class SensorModel_ {
00044 public:
00045   SensorModel_();
00046   virtual ~SensorModel_();
00047 
00048   virtual void Load(sdf::ElementPtr _sdf, const std::string& prefix = std::string());
00049 
00050   virtual T operator()(const T& value) const { return value * scale_error + current_error_; }
00051   virtual T operator()(const T& value, double dt) { return value * scale_error + update(dt); }
00052 
00053   virtual T update(double dt);
00054   virtual void reset();
00055   virtual void reset(const T& value);
00056 
00057   virtual const T& getCurrentError() const { return current_error_; }
00058   virtual T getCurrentBias() const { return current_drift_ + offset; }
00059   virtual const T& getCurrentDrift() const { return current_drift_; }
00060   virtual const T& getScaleError() const { return scale_error; }
00061 
00062   virtual void setCurrentDrift(const T& new_drift) { current_drift_ = new_drift; }
00063 
00064   virtual void dynamicReconfigureCallback(SensorModelConfig &config, uint32_t level);
00065 
00066 private:
00067   virtual bool LoadImpl(sdf::ElementPtr _element, T& _value);
00068 
00069 public:
00070   T offset;
00071   T drift;
00072   T drift_frequency;
00073   T gaussian_noise;
00074   T scale_error;
00075 
00076 private:
00077   T current_drift_;
00078   T current_error_;
00079 };
00080 
00081 template <typename T>
00082 SensorModel_<T>::SensorModel_()
00083   : offset()
00084   , drift()
00085   , drift_frequency()
00086   , gaussian_noise()
00087 {
00088   drift_frequency = 1.0/3600.0; // time constant 1h
00089   scale_error = 1.0;
00090   reset();
00091 }
00092 
00093 template <typename T>
00094 SensorModel_<T>::~SensorModel_()
00095 {
00096 }
00097 
00098 template <typename T>
00099 void SensorModel_<T>::Load(sdf::ElementPtr _sdf, const std::string& prefix)
00100 {
00101   std::string _offset, _drift, _drift_frequency, _gaussian_noise, _scale_error;
00102 
00103   if (prefix.empty()) {
00104     _offset              = "offset";
00105     _drift               = "drift";
00106     _drift_frequency     = "driftFrequency";
00107     _gaussian_noise      = "gaussianNoise";
00108     _scale_error         = "scaleError";
00109   } else {
00110     _offset              = prefix + "Offset";
00111     _drift               = prefix + "Drift";
00112     _drift_frequency     = prefix + "DriftFrequency";
00113     _gaussian_noise      = prefix + "GaussianNoise";
00114     _scale_error         = prefix + "ScaleError";
00115   }
00116 
00117   if (_sdf->HasElement(_offset))              LoadImpl(_sdf->GetElement(_offset), offset);
00118   if (_sdf->HasElement(_drift))               LoadImpl(_sdf->GetElement(_drift), drift);
00119   if (_sdf->HasElement(_drift_frequency))     LoadImpl(_sdf->GetElement(_drift_frequency), drift_frequency);
00120   if (_sdf->HasElement(_gaussian_noise))      LoadImpl(_sdf->GetElement(_gaussian_noise), gaussian_noise);
00121   if (_sdf->HasElement(_scale_error))         LoadImpl(_sdf->GetElement(_scale_error), scale_error);
00122 
00123   reset();
00124 }
00125 
00126 template <typename T>
00127 bool SensorModel_<T>::LoadImpl(sdf::ElementPtr _element, T& _value) {
00128   if (!_element->GetValue()) return false;
00129   return _element->GetValue()->Get(_value);
00130 }
00131 
00132 namespace {
00133   template <typename T>
00134   static inline T SensorModelGaussianKernel(T mu, T sigma)
00135   {
00136     // using Box-Muller transform to generate two independent standard normally distributed normal variables
00137     // see wikipedia
00138     T U = (T)rand()/(T)RAND_MAX; // normalized uniform random variable
00139     T V = (T)rand()/(T)RAND_MAX; // normalized uniform random variable
00140     T X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
00141     X = sigma * X + mu;
00142     return X;
00143   }
00144 
00145   template <typename T>
00146   static inline T SensorModelInternalUpdate(T& current_drift, T drift, T drift_frequency, T offset, T gaussian_noise, double dt)
00147   {
00148     // current_drift = current_drift - dt * (current_drift * drift_frequency + SensorModelGaussianKernel(T(), sqrt(2*drift_frequency)*drift));
00149     current_drift = exp(-dt * drift_frequency) * current_drift + dt * SensorModelGaussianKernel(T(), sqrt(2*drift_frequency)*drift);
00150     return offset + current_drift + SensorModelGaussianKernel(T(), gaussian_noise);
00151   }
00152 }
00153 
00154 template <typename T>
00155 T SensorModel_<T>::update(double dt)
00156 {
00157   for(std::size_t i = 0; i < current_error_.size(); ++i) current_error_[i] = SensorModelInternalUpdate(current_drift_[i], drift[i], drift_frequency[i], offset[i], gaussian_noise[i], dt);
00158   return current_error_;
00159 }
00160 
00161 template <>
00162 double SensorModel_<double>::update(double dt)
00163 {
00164   current_error_ = SensorModelInternalUpdate(current_drift_, drift, drift_frequency, offset, gaussian_noise, dt);
00165   return current_error_;
00166 }
00167 
00168 template <>
00169 math::Vector3 SensorModel_<math::Vector3>::update(double dt)
00170 {
00171   current_error_.x = SensorModelInternalUpdate(current_drift_.x, drift.x, drift_frequency.x, offset.x, gaussian_noise.x, dt);
00172   current_error_.y = SensorModelInternalUpdate(current_drift_.y, drift.y, drift_frequency.y, offset.y, gaussian_noise.y, dt);
00173   current_error_.z = SensorModelInternalUpdate(current_drift_.z, drift.z, drift_frequency.z, offset.z, gaussian_noise.z, dt);
00174   return current_error_;
00175 }
00176 
00177 template <typename T>
00178 void SensorModel_<T>::reset()
00179 {
00180   for(std::size_t i = 0; i < current_drift_.size(); ++i) current_drift_[i] = SensorModelGaussianKernel(T::value_type(), drift[i]);
00181   current_error_ = T();
00182 }
00183 
00184 template <>
00185 void SensorModel_<double>::reset()
00186 {
00187   current_drift_ = SensorModelGaussianKernel(0.0, drift);
00188   current_error_ = 0.0;
00189 }
00190 
00191 template <>
00192 void SensorModel_<math::Vector3>::reset()
00193 {
00194   current_drift_.x = SensorModelGaussianKernel(0.0, drift.x);
00195   current_drift_.y = SensorModelGaussianKernel(0.0, drift.y);
00196   current_drift_.z = SensorModelGaussianKernel(0.0, drift.z);
00197   current_error_ = math::Vector3();
00198 }
00199 
00200 template <typename T>
00201 void SensorModel_<T>::reset(const T& value)
00202 {
00203   current_drift_ = value;
00204   current_error_ = T();
00205 }
00206 
00207 namespace helpers {
00208   template <typename T> struct scalar_value { static double toDouble(const T &orig) { return orig; } };
00209   template <typename T> struct scalar_value<std::vector<T> > { static double toDouble(const std::vector<T> &orig) { return (double) std::accumulate(orig.begin(), orig.end()) / orig.size(); } };
00210   template <> struct scalar_value<math::Vector3> { static double toDouble(const math::Vector3 &orig) { return (orig.x + orig.y + orig.z) / 3; } };
00211 }
00212 
00213 template <typename T>
00214 void SensorModel_<T>::dynamicReconfigureCallback(SensorModelConfig &config, uint32_t level)
00215 {
00216   if (level == 1) {
00217     gaussian_noise  = config.gaussian_noise;
00218     offset          = config.offset;
00219     drift           = config.drift;
00220     drift_frequency = config.drift_frequency;
00221     scale_error     = config.scale_error;
00222   } else {
00223     config.gaussian_noise  = helpers::scalar_value<T>::toDouble(gaussian_noise);
00224     config.offset          = helpers::scalar_value<T>::toDouble(offset);
00225     config.drift           = helpers::scalar_value<T>::toDouble(drift);
00226     config.drift_frequency = helpers::scalar_value<T>::toDouble(drift_frequency);
00227     config.scale_error     = helpers::scalar_value<T>::toDouble(scale_error);
00228   }
00229 }
00230 
00231 typedef SensorModel_<double> SensorModel;
00232 typedef SensorModel_<math::Vector3> SensorModel3;
00233 
00234 }
00235 
00236 #endif // HECTOR_GAZEBO_PLUGINS_SENSOR_MODEL_H


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Sat Jun 8 2019 19:52:36