sr_math_utils.hpp
Go to the documentation of this file.
1
27 #ifndef _SR_MATH_UTILS_HPP_
28 #define _SR_MATH_UTILS_HPP_
29
30 #include <boost/random/mersenne_twister.hpp>
31 #include <boost/random/uniform_int.hpp>
32 #include <boost/random/uniform_real.hpp>
34
35 #include <utility>
36
37 #include <ros/ros.h>
38
39 namespace sr_math_utils
40 {
41 static const double pi = 3.14159265;
42
50 static inline double to_rad(double degrees)
51 {
52  return degrees * 0.017453292519943295;
53 }
54
62 static inline double to_degrees(double rad)
63 {
65 }
66
67 static inline int ipow(int base, int exp)
68 {
69  int result = 1;
70  while (exp)
71  {
72  if (exp & 1)
73  {
74  result *= base;
75  }
76  exp >>= 1;
77  base *= base;
78  }
79
80  return result;
81 }
82
84 {
85  if (bit_mask & (((int64_t) 1) << index))
86  {
87  return true;
88  }
89  else
90  {
91  return false;
92  }
93 }
94
96 {
98 }
99
111 static inline uint64_t counter_with_overflow(uint64_t full_value, uint16_t new_value)
112 {
113  uint16_t last_value = full_value & 0xFFFF; // Split the full value into the lower part
114  full_value &= (uint64_t) 0xFFFFFFFFFFFF0000LL; // and the overflow part
115
116  if (new_value < last_value)
117  { // if we overflowed
118  full_value += (uint64_t) 0x0000000000010000LL;
119  } // then count the overflow
120
121  full_value |= (uint64_t) new_value; // replace the bottom 16 bits with their new value
122
123  return full_value;
124 }
125
139 static inline double linear_interpolate_(double x,
140  double x0, double y0,
141  double x1, double y1)
142 {
143  // y1 - y0
144  double y = y1 - y0;
145  // (y1 - y0) / (x1 - x0)
146  y /= (x1 - x0);
147  // (x-x0)*((y1-y0)/(x1-x0))
148  y *= (x - x0);
149  // y0 + (x-x0)*((y1-y0)/(x1-x0))
150  y += y0;
151
152  return y;
153 }
154
155 namespace filters
156 {
157
159 {
160 public:
161  explicit LowPassFilter(double tau = 0.05)
162  : is_first(true), dt(0.0),
163  timestamp_1(0.0), q_prev(0.0),
164  tau(tau), value_derivative(0.0, 0.0)
165  {
166  };
167
178  std::pair<double, double> compute(double q, double timestamp)
179  {
180  if (is_first)
181  {
182  q_prev = q;
183  // initializing dt to 1ms
184  dt = 0.001;
185
186  is_first = false;
187  }
188  else
189  {
190  dt = timestamp - timestamp_1;
191  }
192
193  double alpha = 0.0;
194  if (tau > 0.0 && dt > 0.0)
195  alpha = exp(-dt / tau);
196
197  // filtering the input
198  value_derivative.first = alpha * value_derivative.first + (1 - alpha) * q;
199  // filtering the derivative
200  value_derivative.second = alpha * value_derivative.second + (1 - alpha) / dt * (value_derivative.first - q_prev);
201
202  q_prev = value_derivative.first;
203  timestamp_1 = timestamp;
204
205  return value_derivative;
206  };
207
208 private:
209  bool is_first;
211
212  std::pair<double, double> value_derivative;
213 };
214
221 {
222 public:
223  explicit AlphaBetaFilter(double alpha = 0.85, double beta = 0.05)
224  : a(alpha), b(beta),
225  xk_1(0.0), vk_1(0.0), xk(0.0), vk(0.0), rk(0.0),
226  dt(0.0), timestamp_1(0.0)
227  {
228  };
229
240  std::pair<double, double> compute(double xm, double timestamp)
241  {
242  dt = timestamp - timestamp_1;
243
244  xk = xk_1 + (vk_1 * dt);
245  vk = vk_1;
246
247  rk = xm - xk;
248
249  xk += a * rk;
250  vk += (b * rk) / dt;
251
252  value_derivative.first = xk;
253  value_derivative.second = vk;
254
255  xk_1 = xk;
256  vk_1 = vk;
257  timestamp_1 = timestamp;
258
259  return value_derivative;
260  };
261
262 protected:
263  double a, b;
264  double xk_1, vk_1, xk, vk, rk;
265  double dt, timestamp_1;
266
267  std::pair<double, double> value_derivative;
268 };
269 } // namespace filters
270
278 static inline int sign(double x)
279 {
280  return x < 0.0 ? -1 : 1;
281 }
283 // FILTERS //
285
286
288 // RANDOM //
290
295 class Random_
296 {
297 public:
299  dist(boost::uniform_real<>(0.0, 1.0))
300  {
301  };
302
310  template<typename T>
311  T generate(T min = static_cast<T> (0), T max = static_cast<T> (1))
312  {
313  return static_cast<T> (min + dist(gen) * (max - min));
314  }
315
316 protected:
317  boost::mt19937 gen;
318  boost::uniform_real<> dist;
319 };
320
335 } // namespace sr_math_utils
336
337 /* For the emacs weenies in the crowd.
338 Local Variables:
339  c-basic-offset: 2
340 End:
341  */
342
343 #endif
gen
static int sign(double x)
static uint64_t counter_with_overflow(uint64_t full_value, uint16_t new_value)
TFSIMD_FORCE_INLINE const tfScalar & y() const
T generate(T min=static_cast< T >(0), T max=static_cast< T >(1))
std::pair< double, double > value_derivative
static int ipow(int base, int exp)
boost::uniform_real dist
std::pair< double, double > compute(double xm, double timestamp)
static const double pi
std::pair< double, double > value_derivative
static double linear_interpolate_(double x, double x0, double y0, double x1, double y1)