sensor_model.h
Go to the documentation of this file.
1 //=================================================================================================
2 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
3 // All rights reserved.
4 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 // * Neither the name of the Flight Systems and Automatic Control group,
13 // TU Darmstadt, nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without
15 // specific prior written permission.
16 
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //=================================================================================================
28 
29 #ifndef HECTOR_GAZEBO_PLUGINS_SENSOR_MODEL_H
30 #define HECTOR_GAZEBO_PLUGINS_SENSOR_MODEL_H
31 
32 #include <sdf/sdf.hh>
33 #if (GAZEBO_MAJOR_VERSION < 8)
34 #include <gazebo/math/Vector3.hh>
35 #endif
36 
37 #include <hector_gazebo_plugins/SensorModelConfig.h>
38 #include <numeric>
39 
40 namespace gazebo {
41 
42 using hector_gazebo_plugins::SensorModelConfig;
43 
44 template <typename T>
45 class SensorModel_ {
46 public:
47  SensorModel_();
48  virtual ~SensorModel_();
49 
50  virtual void Load(sdf::ElementPtr _sdf, const std::string& prefix = std::string());
51 
52  virtual T operator()(const T& value) const { return value * scale_error + current_error_; }
53  virtual T operator()(const T& value, double dt) { return value * scale_error + update(dt); }
54 
55  virtual T update(double dt);
56  virtual void reset();
57  virtual void reset(const T& value);
58 
59  virtual const T& getCurrentError() const { return current_error_; }
60  virtual T getCurrentBias() const { return current_drift_ + offset; }
61  virtual const T& getCurrentDrift() const { return current_drift_; }
62  virtual const T& getScaleError() const { return scale_error; }
63 
64  virtual void setCurrentDrift(const T& new_drift) { current_drift_ = new_drift; }
65 
66  virtual void dynamicReconfigureCallback(SensorModelConfig &config, uint32_t level);
67 
68 private:
69  virtual bool LoadImpl(sdf::ElementPtr _element, T& _value);
70 
71 public:
72  T offset;
73  T drift;
77 
78 private:
81 };
82 
83 template <typename T>
85  : offset()
86  , drift()
87  , drift_frequency()
88  , gaussian_noise()
89 {
90  drift_frequency = 1.0/3600.0; // time constant 1h
91  scale_error = 1.0;
92  reset();
93 }
94 
95 template <typename T>
97 {
98 }
99 
100 template <typename T>
101 void SensorModel_<T>::Load(sdf::ElementPtr _sdf, const std::string& prefix)
102 {
103  std::string _offset, _drift, _drift_frequency, _gaussian_noise, _scale_error;
104 
105  if (prefix.empty()) {
106  _offset = "offset";
107  _drift = "drift";
108  _drift_frequency = "driftFrequency";
109  _gaussian_noise = "gaussianNoise";
110  _scale_error = "scaleError";
111  } else {
112  _offset = prefix + "Offset";
113  _drift = prefix + "Drift";
114  _drift_frequency = prefix + "DriftFrequency";
115  _gaussian_noise = prefix + "GaussianNoise";
116  _scale_error = prefix + "ScaleError";
117  }
118 
119  if (_sdf->HasElement(_offset)) LoadImpl(_sdf->GetElement(_offset), offset);
120  if (_sdf->HasElement(_drift)) LoadImpl(_sdf->GetElement(_drift), drift);
121  if (_sdf->HasElement(_drift_frequency)) LoadImpl(_sdf->GetElement(_drift_frequency), drift_frequency);
122  if (_sdf->HasElement(_gaussian_noise)) LoadImpl(_sdf->GetElement(_gaussian_noise), gaussian_noise);
123  if (_sdf->HasElement(_scale_error)) LoadImpl(_sdf->GetElement(_scale_error), scale_error);
124 
125  reset();
126 }
127 
128 template <typename T>
129 bool SensorModel_<T>::LoadImpl(sdf::ElementPtr _element, T& _value) {
130  if (!_element->GetValue()) return false;
131  return _element->GetValue()->Get(_value);
132 }
133 
134 namespace {
135  template <typename T>
136  static inline T SensorModelGaussianKernel(T mu, T sigma)
137  {
138  // using Box-Muller transform to generate two independent standard normally distributed normal variables
139  // see wikipedia
140  T U = (T)rand()/(T)RAND_MAX; // normalized uniform random variable
141  T V = (T)rand()/(T)RAND_MAX; // normalized uniform random variable
142  T X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
143  X = sigma * X + mu;
144  return X;
145  }
146 
147  template <typename T>
148  static inline T SensorModelInternalUpdate(T& current_drift, T drift, T drift_frequency, T offset, T gaussian_noise, double dt)
149  {
150  // current_drift = current_drift - dt * (current_drift * drift_frequency + SensorModelGaussianKernel(T(), sqrt(2*drift_frequency)*drift));
151  current_drift = exp(-dt * drift_frequency) * current_drift + dt * SensorModelGaussianKernel(T(), sqrt(2*drift_frequency)*drift);
152  return offset + current_drift + SensorModelGaussianKernel(T(), gaussian_noise);
153  }
154 }
155 
156 template <typename T>
158 {
159  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);
160  return current_error_;
161 }
162 
163 template <>
165 {
166  current_error_ = SensorModelInternalUpdate(current_drift_, drift, drift_frequency, offset, gaussian_noise, dt);
167  return current_error_;
168 }
169 
170 #if (GAZEBO_MAJOR_VERSION >= 8)
171 template <>
172 ignition::math::Vector3d SensorModel_<ignition::math::Vector3d>::update(double dt)
173 {
174  current_error_.X() = SensorModelInternalUpdate(current_drift_.X(), drift.X(), drift_frequency.X(), offset.X(), gaussian_noise.X(), dt);
175  current_error_.Y() = SensorModelInternalUpdate(current_drift_.Y(), drift.Y(), drift_frequency.Y(), offset.Y(), gaussian_noise.Y(), dt);
176  current_error_.Z() = SensorModelInternalUpdate(current_drift_.Z(), drift.Z(), drift_frequency.Z(), offset.Z(), gaussian_noise.Z(), dt);
177  return current_error_;
178 }
179 #else
180 template <>
181 math::Vector3 SensorModel_<math::Vector3>::update(double dt)
182 {
183  current_error_.x = SensorModelInternalUpdate(current_drift_.x, drift.x, drift_frequency.x, offset.x, gaussian_noise.x, dt);
184  current_error_.y = SensorModelInternalUpdate(current_drift_.y, drift.y, drift_frequency.y, offset.y, gaussian_noise.y, dt);
185  current_error_.z = SensorModelInternalUpdate(current_drift_.z, drift.z, drift_frequency.z, offset.z, gaussian_noise.z, dt);
186  return current_error_;
187 }
188 #endif
189 
190 template <typename T>
192 {
193  for(std::size_t i = 0; i < current_drift_.size(); ++i) current_drift_[i] = SensorModelGaussianKernel(T::value_type(), drift[i]);
194  current_error_ = T();
195 }
196 
197 template <>
199 {
200  current_drift_ = SensorModelGaussianKernel(0.0, drift);
201  current_error_ = 0.0;
202 }
203 
204 #if (GAZEBO_MAJOR_VERSION >= 8)
205 template <>
207 {
208  current_drift_.X() = SensorModelGaussianKernel(0.0, drift.X());
209  current_drift_.Y() = SensorModelGaussianKernel(0.0, drift.Y());
210  current_drift_.Z() = SensorModelGaussianKernel(0.0, drift.Z());
211  current_error_ = ignition::math::Vector3d();
212 }
213 #else
214 template <>
216 {
217  current_drift_.x = SensorModelGaussianKernel(0.0, drift.x);
218  current_drift_.y = SensorModelGaussianKernel(0.0, drift.y);
219  current_drift_.z = SensorModelGaussianKernel(0.0, drift.z);
220  current_error_ = math::Vector3();
221 }
222 #endif
223 
224 template <typename T>
225 void SensorModel_<T>::reset(const T& value)
226 {
227  current_drift_ = value;
228  current_error_ = T();
229 }
230 
231 namespace helpers {
232  template <typename T> struct scalar_value { static double toDouble(const T &orig) { return orig; } };
233  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(); } };
234 #if (GAZEBO_MAJOR_VERSION >= 8)
235  template <> struct scalar_value<ignition::math::Vector3d> { static double toDouble(const ignition::math::Vector3d &orig) { return (orig.X() + orig.Y() + orig.Z()) / 3; } };
236 #else
237  template <> struct scalar_value<math::Vector3> { static double toDouble(const math::Vector3 &orig) { return (orig.x + orig.y + orig.z) / 3; } };
238 #endif
239 }
240 
241 template <typename T>
242 void SensorModel_<T>::dynamicReconfigureCallback(SensorModelConfig &config, uint32_t level)
243 {
244  if (level == 1) {
245  gaussian_noise = config.gaussian_noise;
246  offset = config.offset;
247  drift = config.drift;
248  drift_frequency = config.drift_frequency;
249  scale_error = config.scale_error;
250  } else {
251  config.gaussian_noise = helpers::scalar_value<T>::toDouble(gaussian_noise);
254  config.drift_frequency = helpers::scalar_value<T>::toDouble(drift_frequency);
255  config.scale_error = helpers::scalar_value<T>::toDouble(scale_error);
256  }
257 }
258 
260 #if (GAZEBO_MAJOR_VERSION >= 8)
262 #else
264 #endif
265 
266 }
267 
268 #endif // HECTOR_GAZEBO_PLUGINS_SENSOR_MODEL_H
virtual const T & getCurrentDrift() const
Definition: sensor_model.h:61
virtual T update(double dt)
Definition: sensor_model.h:157
virtual void setCurrentDrift(const T &new_drift)
Definition: sensor_model.h:64
virtual void Load(sdf::ElementPtr _sdf, const std::string &prefix=std::string())
Definition: sensor_model.h:101
virtual T operator()(const T &value) const
Definition: sensor_model.h:52
virtual ~SensorModel_()
Definition: sensor_model.h:96
virtual T getCurrentBias() const
Definition: sensor_model.h:60
virtual T operator()(const T &value, double dt)
Definition: sensor_model.h:53
virtual bool LoadImpl(sdf::ElementPtr _element, T &_value)
Definition: sensor_model.h:129
virtual void dynamicReconfigureCallback(SensorModelConfig &config, uint32_t level)
Definition: sensor_model.h:242
TFSIMD_FORCE_INLINE Vector3()
SensorModel_< math::Vector3 > SensorModel3
Definition: sensor_model.h:263
static double toDouble(const T &orig)
Definition: sensor_model.h:232
virtual void reset()
Definition: sensor_model.h:191
SensorModel_< double > SensorModel
Definition: sensor_model.h:259
static double toDouble(const math::Vector3 &orig)
Definition: sensor_model.h:237
virtual const T & getCurrentError() const
Definition: sensor_model.h:59
static double toDouble(const std::vector< T > &orig)
Definition: sensor_model.h:233
virtual const T & getScaleError() const
Definition: sensor_model.h:62


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Fri Feb 5 2021 03:48:30