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;
00041 
00049   static inline double to_rad(double degrees)
00050   {
00051     return degrees * 0.017453292519943295;
00052   }
00053 
00061   static inline double to_degrees(double rad)
00062   {
00063     return rad * 57.295779513082323;
00064   }
00065 
00066   static inline int ipow(int base, int exp)
00067   {
00068     int result = 1;
00069     while (exp)
00070     {
00071       if (exp & 1)
00072         result *= base;
00073       exp >>= 1;
00074       base *= base;
00075     }
00076 
00077     return result;
00078   }
00079 
00080   static inline bool is_bit_mask_index_true(long int bit_mask, int index)
00081   {
00082     if ( bit_mask & (((long int)1)<<index) )
00083       return true;
00084     else
00085       return false;
00086   }
00087 
00088   static inline bool is_bit_mask_index_false(long int bit_mask, int index)
00089   {
00090     return !(is_bit_mask_index_true(bit_mask, index));
00091   }
00092 
00104   static inline uint64_t counter_with_overflow(uint64_t full_value, uint16_t new_value)
00105   {
00106     uint16_t last_value = full_value &    0xFFFF;       // Split the full value into the lower part
00107     full_value   &= (uint64_t)0xFFFFFFFFFFFF0000LL;     // and the overflow part
00108 
00109     if( new_value < last_value)                         // if we overflowed
00110       full_value += (uint64_t)0x0000000000010000LL;     // then count the overflow
00111 
00112     full_value   |= (uint64_t)new_value;                // replace the bottom 16 bits with their new value
00113 
00114     return full_value;
00115   }
00116 
00117 
00131   static inline double linear_interpolate_(double x,
00132                                            double x0, double y0,
00133                                            double x1, double y1)
00134   {
00135     //y1 - y0
00136     double y = y1 - y0;
00137     // (y1 - y0) / (x1 - x0)
00138     y /= (x1 - x0);
00139     //  (x-x0)*((y1-y0)/(x1-x0))
00140     y *= (x - x0);
00141     //  y0 + (x-x0)*((y1-y0)/(x1-x0))
00142     y += y0;
00143 
00144     return y;
00145   }
00146 
00154   static inline int sign(double x)
00155   {
00156     return x < 0.0 ? -1 : 1;
00157   }
00158 
00160   //  FILTERS  //
00162   namespace filters
00163   {
00164 
00165     class LowPassFilter
00166     {
00167     public:
00168       LowPassFilter(double tau= 0.05)
00169         : is_first(true), dt(0.0),
00170           timestamp_1(0.0), q_prev(0.0)
00171       {
00172         this->tau = tau;
00173         value_derivative.first = 0.0;
00174         value_derivative.second = 0.0;
00175       };
00176 
00187       std::pair<double, double> compute(double q, double timestamp)
00188       {
00189         if( is_first )
00190         {
00191           q_prev = q;
00192           //initializing dt to 1ms
00193           dt = 0.001;
00194 
00195           is_first = false;
00196         }
00197         else
00198           dt = timestamp - timestamp_1;
00199 
00200         double alpha = exp(-dt / tau);
00201 
00202         //filtering the input
00203         value_derivative.first = alpha * value_derivative.first + (1 - alpha)*q;
00204         //filtering the derivative
00205         value_derivative.second = alpha * value_derivative.second + (1 - alpha) / dt * (q - q_prev);
00206 
00207         q_prev = q;
00208         timestamp_1 = timestamp;
00209 
00210         return value_derivative;
00211       };
00212 
00213     private:
00214       bool is_first;
00215       double tau, dt, timestamp_1, q_prev;
00216 
00217       std::pair<double, double> value_derivative;
00218     };
00219 
00225     class AlphaBetaFilter
00226     {
00227     public:
00228       AlphaBetaFilter(double alpha = 0.85, double beta = 0.05)
00229         : a(alpha), b(beta),
00230           xk_1(0.0), vk_1(0.0), xk(0.0), vk(0.0), rk(0.0),
00231           dt(0.0), timestamp_1(0.0)
00232       {};
00233 
00234       ~AlphaBetaFilter()
00235       {};
00236 
00247       std::pair<double, double> compute(double xm, double timestamp)
00248       {
00249         dt = timestamp - timestamp_1;
00250 
00251         xk = xk_1 + ( vk_1 * dt );
00252         vk = vk_1;
00253 
00254         rk = xm - xk;
00255 
00256         xk += a * rk;
00257         vk += ( b * rk ) / dt;
00258 
00259         value_derivative.first = xk;
00260         value_derivative.second = vk;
00261 
00262         xk_1 = xk;
00263         vk_1 = vk;
00264         timestamp_1 = timestamp;
00265 
00266         return value_derivative;
00267       };
00268 
00269     protected:
00270       double a, b;
00271       double xk_1, vk_1, xk, vk, rk;
00272       double dt, timestamp_1;
00273 
00274       std::pair<double, double> value_derivative;
00275     };
00276   };
00277 
00278 
00280   //  RANDOM   //
00282 
00287   class Random_
00288   {
00289   public:
00290     Random_()
00291     {
00292       dist = boost::uniform_real<>(0.0, 1.0);
00293     };
00294 
00295     ~Random_()
00296     {};
00297 
00305     template <typename T>
00306     T generate(T min = static_cast<T>(0), T max = static_cast<T>(1))
00307     {
00308       return static_cast<T>( min + dist(gen) * (max - min)  );
00309     }
00310 
00311   protected:
00312     boost::mt19937 gen;
00313     boost::uniform_real<> dist;
00314   };
00315 
00329   typedef boost::detail::thread::singleton < class Random_ > Random;
00330 }
00331 
00332 /* For the emacs weenies in the crowd.
00333 Local Variables:
00334    c-basic-offset: 2
00335 End:
00336 */
00337 
00338 #endif


sr_utilities
Author(s): Ugo Cupcic
autogenerated on Mon Oct 6 2014 07:42:15