sr_math_utils.hpp
Go to the documentation of this file.
00001 
00027 #ifndef _SR_MATH_UTILS_HPP_
00028 #define _SR_MATH_UTILS_HPP_
00029 
00030 #include <boost/random/mersenne_twister.hpp>
00031 #include <boost/random/uniform_int.hpp>
00032 #include <boost/random/uniform_real.hpp>
00033 #include <boost/thread/detail/singleton.hpp>
00034 
00035 
00036 #include <ros/ros.h>
00037 
00038 namespace sr_math_utils
00039 {
00040 static const double pi = 3.14159265;
00048 static inline double to_rad(double degrees)
00049 {
00050   return degrees * 0.017453292519943295;
00051 }
00059 static inline double to_degrees(double rad)
00060 {
00061   return rad * 57.295779513082323;
00062 }
00063 static inline int ipow(int base, int exp)
00064 {
00065   int result = 1;
00066   while (exp)
00067   {
00068     if (exp & 1)
00069       result *= base;
00070     exp >>= 1;
00071     base *= base;
00072   }
00073 
00074   return result;
00075 }
00076 static inline bool is_bit_mask_index_true(long int bit_mask, int index)
00077 {
00078   if (bit_mask & (((long int) 1) << index))
00079     return true;
00080   else
00081     return false;
00082 }
00083 static inline bool is_bit_mask_index_false(long int bit_mask, int index)
00084 {
00085   return !(is_bit_mask_index_true(bit_mask, index));
00086 }
00098 static inline uint64_t counter_with_overflow(uint64_t full_value, uint16_t new_value)
00099 {
00100   uint16_t last_value = full_value & 0xFFFF; // Split the full value into the lower part
00101   full_value &= (uint64_t) 0xFFFFFFFFFFFF0000LL; // and the overflow part
00102 
00103   if (new_value < last_value) // if we overflowed
00104     full_value += (uint64_t) 0x0000000000010000LL; // then count the overflow
00105 
00106   full_value |= (uint64_t) new_value; // replace the bottom 16 bits with their new value
00107 
00108   return full_value;
00109 }
00123 static inline double linear_interpolate_(double x,
00124                                          double x0, double y0,
00125                                          double x1, double y1)
00126 {
00127   //y1 - y0
00128   double y = y1 - y0;
00129   // (y1 - y0) / (x1 - x0)
00130   y /= (x1 - x0);
00131   //  (x-x0)*((y1-y0)/(x1-x0))
00132   y *= (x - x0);
00133   //  y0 + (x-x0)*((y1-y0)/(x1-x0))
00134   y += y0;
00135 
00136   return y;
00137 }
00145 static inline int sign(double x)
00146 {
00147   return x < 0.0 ? -1 : 1;
00148 }
00149 
00151 //  FILTERS  //
00153 namespace filters
00154 {
00155 
00156 class LowPassFilter
00157 {
00158 public:
00159   LowPassFilter(double tau = 0.05)
00160     : is_first(true), dt(0.0),
00161     timestamp_1(0.0), q_prev(0.0),
00162     tau(tau), value_derivative(0.0, 0.0)
00163   {
00164   };
00175   std::pair<double, double> compute(double q, double timestamp)
00176   {
00177     if (is_first)
00178     {
00179       q_prev = q;
00180       //initializing dt to 1ms
00181       dt = 0.001;
00182 
00183       is_first = false;
00184     }
00185     else
00186       dt = timestamp - timestamp_1;
00187 
00188     double alpha = exp(-dt / tau);
00189 
00190     //filtering the input
00191     value_derivative.first = alpha * value_derivative.first + (1 - alpha) * q;
00192     //filtering the derivative
00193     value_derivative.second = alpha * value_derivative.second + (1 - alpha) / dt * (q - q_prev);
00194 
00195     q_prev = q;
00196     timestamp_1 = timestamp;
00197 
00198     return value_derivative;
00199   };
00200 
00201 private:
00202   bool is_first;
00203   double tau, dt, timestamp_1, q_prev;
00204 
00205   std::pair<double, double> value_derivative;
00206 };
00207 
00213 class AlphaBetaFilter
00214 {
00215 public:
00216   AlphaBetaFilter(double alpha = 0.85, double beta = 0.05)
00217     : a(alpha), b(beta),
00218     xk_1(0.0), vk_1(0.0), xk(0.0), vk(0.0), rk(0.0),
00219     dt(0.0), timestamp_1(0.0)
00220   {
00221   };
00232   std::pair<double, double> compute(double xm, double timestamp)
00233   {
00234     dt = timestamp - timestamp_1;
00235 
00236     xk = xk_1 + (vk_1 * dt);
00237     vk = vk_1;
00238 
00239     rk = xm - xk;
00240 
00241     xk += a * rk;
00242     vk += (b * rk) / dt;
00243 
00244     value_derivative.first = xk;
00245     value_derivative.second = vk;
00246 
00247     xk_1 = xk;
00248     vk_1 = vk;
00249     timestamp_1 = timestamp;
00250 
00251     return value_derivative;
00252   };
00253 
00254 protected:
00255   double a, b;
00256   double xk_1, vk_1, xk, vk, rk;
00257   double dt, timestamp_1;
00258 
00259   std::pair<double, double> value_derivative;
00260 };
00261 };
00262 
00263 
00265 //  RANDOM   //
00267 
00272 class Random_
00273 {
00274 public:
00275   Random_() :
00276     dist(boost::uniform_real<>(0.0, 1.0))
00277   {
00278   };
00286   template <typename T>
00287   T generate(T min = static_cast<T> (0), T max = static_cast<T> (1))
00288   {
00289     return static_cast<T> (min + dist(gen) * (max - min));
00290   }
00291 
00292 protected:
00293   boost::mt19937 gen;
00294   boost::uniform_real<> dist;
00295 };
00296 
00310 typedef boost::detail::thread::singleton < class Random_ > Random;
00311 }
00312 
00313 /* For the emacs weenies in the crowd.
00314 Local Variables:
00315    c-basic-offset: 2
00316 End:
00317  */
00318 
00319 #endif


sr_utilities
Author(s): Ugo Cupcic
autogenerated on Fri Aug 21 2015 12:24:47