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 <gazebo/sdf/sdf.hh>
00033 
00034 namespace gazebo {
00035 
00036 template <typename T>
00037 class SensorModel_ {
00038 public:
00039   SensorModel_();
00040   virtual ~SensorModel_();
00041 
00042   virtual void Load(sdf::ElementPtr _sdf, const std::string& prefix = std::string());
00043 
00044   virtual T operator()(const T& value) const { return value + current_error_; }
00045   virtual T operator()(const T& value, double dt) { return value + update(dt); }
00046 
00047   virtual T update(double dt);
00048   virtual void reset(const T& value = T());
00049 
00050   virtual const T& getCurrentError() const { return current_error_; }
00051   virtual const T& getCurrentDrift() const { return current_drift_; }
00052 
00053   virtual void setCurrentDrift(const T& new_drift) { current_drift_ = new_drift; }
00054 
00055 private:
00056   virtual bool LoadImpl(sdf::ElementPtr _element, T& _value);
00057 
00058 public:
00059   T offset;
00060   T drift;
00061   T drift_frequency;
00062   T gaussian_noise;
00063 
00064 private:
00065   T current_drift_;
00066   T current_error_;
00067 };
00068 
00069 template <typename T>
00070 SensorModel_<T>::SensorModel_()
00071   : offset()
00072   , drift()
00073   , drift_frequency()
00074   , gaussian_noise()
00075 {
00076   drift_frequency = 1.0/3600.0;
00077   reset();
00078 }
00079 
00080 template <typename T>
00081 SensorModel_<T>::~SensorModel_()
00082 {
00083 }
00084 
00085 template <typename T>
00086 void SensorModel_<T>::Load(sdf::ElementPtr _sdf, const std::string& prefix)
00087 {
00088   std::string _offset, _drift, _drift_frequency, _gaussian_noise;
00089 
00090   if (prefix.empty()) {
00091     _offset          = "offset";
00092     _drift           = "drift";
00093     _drift_frequency = "driftFrequency";
00094     _gaussian_noise  = "gaussianNoise";
00095   } else {
00096     _offset          = prefix + "Offset";
00097     _drift           = prefix + "Drift";
00098     _drift_frequency = prefix + "DriftFrequency";
00099     _gaussian_noise  = prefix + "GaussianNoise";
00100   }
00101 
00102   if (_sdf->HasElement(_offset))          LoadImpl(_sdf->GetElement(_offset), offset);
00103   if (_sdf->HasElement(_drift))           LoadImpl(_sdf->GetElement(_drift), drift);
00104   if (_sdf->HasElement(_drift_frequency)) LoadImpl(_sdf->GetElement(_drift_frequency), drift_frequency);
00105   if (_sdf->HasElement(_gaussian_noise))  LoadImpl(_sdf->GetElement(_gaussian_noise), gaussian_noise);
00106 }
00107 
00108 template <typename T>
00109 bool SensorModel_<T>::LoadImpl(sdf::ElementPtr _element, T& _value) {
00110   if (!_element->GetValue()) return false;
00111   if (_element->GetValue()->IsStr()) {
00112     try {
00113       _value = boost::lexical_cast<T>(_element->GetValue()->GetAsString());
00114       return true;
00115     } catch(boost::bad_lexical_cast&) {
00116       return false;
00117     }
00118   }
00119   return _element->GetValue()->Get(_value);
00120 }
00121 
00122 namespace {
00123   template <typename T>
00124   static inline T SensorModelGaussianKernel(T mu, T sigma)
00125   {
00126     // using Box-Muller transform to generate two independent standard normally disbributed normal variables
00127     // see wikipedia
00128     T U = (T)rand()/(T)RAND_MAX; // normalized uniform random variable
00129     T V = (T)rand()/(T)RAND_MAX; // normalized uniform random variable
00130     T X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
00131     X = sigma * X + mu;
00132     return X;
00133   }
00134 
00135   template <typename T>
00136   static inline T SensorModelInternalUpdate(T& current_drift, T drift, T drift_frequency, T offset, T gaussian_noise, double dt)
00137   {
00138     current_drift = current_drift - dt * (current_drift * drift_frequency + SensorModelGaussianKernel(T(), sqrt(2*drift_frequency)*drift));
00139     return offset + current_drift + SensorModelGaussianKernel(T(), gaussian_noise);
00140   }
00141 }
00142 
00143 template <typename T>
00144 T SensorModel_<T>::update(double dt)
00145 {
00146   for(std::size_t i = 0; i < current_error_.size(); ++i) current_error_[i] = current_error_ = SensorModelInternalUpdate(current_drift_[i], drift[i], drift_frequency[i], offset[i], gaussian_noise[i], dt);
00147   return current_error_;
00148 }
00149 
00150 template <>
00151 double SensorModel_<double>::update(double dt)
00152 {
00153   current_error_ = SensorModelInternalUpdate(current_drift_, drift, drift_frequency, offset, gaussian_noise, dt);
00154   return current_error_;
00155 }
00156 
00157 template <>
00158 math::Vector3 SensorModel_<math::Vector3>::update(double dt)
00159 {
00160   current_error_.x = SensorModelInternalUpdate(current_drift_.x, drift.x, drift_frequency.x, offset.x, gaussian_noise.x, dt);
00161   current_error_.y = SensorModelInternalUpdate(current_drift_.y, drift.y, drift_frequency.y, offset.y, gaussian_noise.y, dt);
00162   current_error_.z = SensorModelInternalUpdate(current_drift_.z, drift.z, drift_frequency.z, offset.z, gaussian_noise.z, dt);
00163   return current_error_;
00164 }
00165 
00166 template <typename T>
00167 void SensorModel_<T>::reset(const T& value)
00168 {
00169   current_drift_ = T();
00170   current_error_ = value;
00171 }
00172 
00173 typedef SensorModel_<double> SensorModel;
00174 typedef SensorModel_<math::Vector3> SensorModel3;
00175 
00176 }
00177 
00178 #endif // HECTOR_GAZEBO_PLUGINS_SENSOR_MODEL_H
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher and Johannes Meyer
autogenerated on Mon Jul 15 2013 16:46:30