Robot Simulator of the Robotics Group for Self-Organization of Control: noisegenerator.h Source File

# noisegenerator.h

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005-2011 LpzRobots development team                    *
    00003  *    Georg Martius  <georg dot martius at web dot de>                     *
    00004  *    Frank Guettler <guettler at informatik dot uni-leipzig dot de        *
    00005  *    Frank Hesse    <frank at nld dot ds dot mpg dot de>                  *
    00006  *    Ralf Der       <ralfder at mis dot mpg dot de>                       *
    00007  *                                                                         *
    00008  *   This program is free software; you can redistribute it and/or modify  *
    00009  *   it under the terms of the GNU General Public License as published by  *
    00010  *   the Free Software Foundation; either version 2 of the License, or     *
    00011  *   (at your option) any later version.                                   *
    00012  *                                                                         *
    00013  *   This program is distributed in the hope that it will be useful,       *
    00014  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00015  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00016  *   GNU General Public License for more details.                          *
    00017  *                                                                         *
    00018  *   You should have received a copy of the GNU General Public License     *
    00019  *   along with this program; if not, write to the                         *
    00020  *   Free Software Foundation, Inc.,                                       *
    00021  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00022  *                                                                         *
    00023  ***************************************************************************/
    00024 #ifndef __NOISEGENERATOR_H
    00025 #define __NOISEGENERATOR_H
    00026
    00027 #include <stdlib.h>
    00028 #include <time.h>
    00029 #include <cmath>
    00030 #include <assert.h>
    00031
    00032 #include "randomgenerator.h"
    00033
    00034 /** Interface and basic class for noise generator.
    00035     It is suitable for single noise channels but also multidimensional noise.
    00036  */
    00037 class NoiseGenerator{
    00038 public:
    00039   NoiseGenerator()
    00040   {
    00041     dimension   = 0;
    00042     randGen     = 0;
    00043     ownRandGen  = false;
    00044   };
    00045
    00046   virtual ~NoiseGenerator()
    00047   {
    00048     if (this->ownRandGen && this->randGen)
    00049     {
    00050       delete this->randGen;
    00051       this->randGen     = 0;
    00052       this->ownRandGen  = false;
    00053     }
    00054   }
    00055
    00056   /** initialization with the the given dimension for multidimensional noise
    00057       @param dimension dimensionality of vectors to be used by add
    00058       @param randGen pointer to a random generator. If zero a new one generated internally
    00059       @see add()
    00060    */
    00061   virtual void init(unsigned int dimension, RandGen* randGen=0)
    00062   {
    00063     this->dimension = dimension;
    00064     if(randGen)
    00065       this->randGen=randGen;
    00066
    00067     else {
    00068       this->randGen=new RandGen();
    00069       this->randGen->init(rand());
    00070       this->ownRandGen=true;
    00071     }
    00072   };
    00073
    00074   /** generate somehow distributed random number parameterized with min and max.
    00075       valid only for ONE random number, use \ref add() for
    00076       adding this kind of noise to several channels
    00077    */
    00078   virtual double generate() = 0;
    00079
    00080   /** adds multidimensional noise to the value field.
    00081       Generic implementation calls generate for each channel.
    00082       Overload this if you need different behavior.
    00083       @param value field where noise is added. Must have length dimension (\ref init())
    00084    */
    00085   virtual void add(double *value, double noiseStrength){
    00086     for (unsigned int i = 0; i < dimension; i++){
    00087       value[i]+=generate()*noiseStrength;
    00088     }
    00089   }
    00090
    00091 protected:
    00092   //generates white (no averaging) uniformly distributed random number between "min" and "max"
    00093   double uniform(double min=-0.1, double max=0.1){
    00094     assert(randGen);
    00095     return( randGen->rand()*(max-min)+min);
    00096   }
    00097   //generates white uniformly distributed random number between in [0,1)
    00098   double uniform01(){
    00099     assert(randGen);
    00100     return randGen->rand();
    00101   }
    00102   unsigned int dimension;
    00103   RandGen* randGen;
    00104   bool ownRandGen;
    00105 };
    00106
    00107 /// generates no noise
    00108 class NoNoise : public NoiseGenerator{
    00109 public:
    00110   NoNoise() {}
    00111   virtual ~NoNoise(){}
    00112   virtual double generate() {
    00113     return 0;
    00114   };
    00115 };
    00116
    00117
    00118 /// generates white (no averaging) uniformly distributed random number between "min" and "max"
    00119 class WhiteUniformNoise : public NoiseGenerator{
    00120 public:
    00121   WhiteUniformNoise() {}
    00122   virtual ~WhiteUniformNoise(){}
    00123   virtual double generate() {
    00124     return uniform(-1,1);
    00125   };
    00126
    00127 };
    00128
    00129 /// generates white and normal distributed random numbers. p1: mean, p2: standard deviation
    00130 /// new parameter definition: p1: min, p2: max. the mean and standard deviation are calculated by this values
    00131 class WhiteNormalNoise : public NoiseGenerator{
    00132 public:
    00133   WhiteNormalNoise(){}
    00134   virtual ~WhiteNormalNoise(){}
    00135   virtual double generate() {
    00136     double x1=uniform01();
    00137     double x2=uniform01();
    00138     return( (sqrt(-2*log(x1)) *cos(2*M_PI*x2)));
    00139   };
    00140   // original version
    00141   //  virtual double generate(double mean, double stddev) {
    00142   //    double x1=uniform(0, 1);
    00143   //    double x2=uniform(0, 1);
    00144   //    return( (sqrt(-2*log(x1)) *cos(2*M_PI*x2))  * stddev + mean) ;
    00145   //  }
    00146
    00147 };
    00148
    00149 /// generated colored noise. This is obtained by using time average of uniform distributed noise.
    00150 class ColorUniformNoise : public NoiseGenerator{
    00151 public:
    00152   /** @param tau time averaging factor (1/window)
    00153       (1: smoothing (white) 0.1: strong color, 0 no noise anymore
    00154   */
    00155   ColorUniformNoise(double tau=0.05)
    00156     : tau(tau){
    00157     sqrttau = sqrt(tau);
    00158     mean1channel=0.0;
    00159     mean=0;
    00160   }
    00161   virtual ~ColorUniformNoise(){ if(mean) free(mean);}
    00162   virtual void init(unsigned int dimension, RandGen* randGen=0){
    00163     NoiseGenerator::init(dimension, randGen);
    00164     mean = (double*)malloc(sizeof(double) * dimension);
    00165     for (unsigned int i=0; i<dimension; i++){
    00166         mean[i]=0.0;
    00167     }
    00168   }
    00169
    00170   virtual double generate() {
    00171     mean1channel+=  sqrttau *  uniform(-1,  +1) - tau *  mean1channel;
    00172     return(mean1channel);
    00173   }
    00174
    00175   /** adds multidimensional noise to the value field.
    00176       @param value field where noise is added. Must have length dimension (\ref init())
    00177       @param min lower bound of interval
    00178       @param max upper bound of interval
    00179    */
    00180   virtual void add(double *value, double noiseStrength){
    00181     for (unsigned int i = 0; i < dimension; i++){
    00182       mean[i]+= sqrttau * uniform(-1,  +1)*noiseStrength - tau *  mean[i];
    00183       value[i]+=mean[i];
    00184     }
    00185   }
    00186
    00187   virtual double getTau(){ return tau;}
    00188   virtual void setTau(double newTau){
    00189     if(newTau >=0 && newTau <= 1){
    00190       tau=newTau;
    00191       sqrttau = sqrt(tau);
    00192     }
    00193   }
    00194
    00195 protected:
    00196   double tau; // smoothing paramter
    00197   double sqrttau; // square root of smoothing parameter
    00198   double* mean;
    00199   double mean1channel;
    00200 };
    00201
    00202 /// like ColorUniformNoise but averaging over normal distributed noise instead.
    00203 class ColorNormalNoise : public WhiteNormalNoise{
    00204 public:
    00205   ColorNormalNoise(double tau=0.05)
    00206     : tau(tau){
    00207     sqrttau = sqrt(tau);
    00208     mean = 0;
    00209     mean1channel=0.0;
    00210   }
    00211
    00212   virtual ~ColorNormalNoise(){if(mean) free(mean);}
    00213
    00214   virtual void init(unsigned int dimension, RandGen* randGen=0){
    00215     WhiteNormalNoise::init(dimension, randGen);
    00216     mean = (double*)malloc(sizeof(double) * dimension);
    00217     for (unsigned int i=0; i<dimension; i++){
    00218         mean[i]=0.0;
    00219     }
    00220   }
    00221
    00222   virtual double generate() { //double stddev, double meanvalue) {
    00223     mean1channel += sqrttau * WhiteNormalNoise::generate() - tau*mean1channel;
    00224     return(mean1channel);
    00225   }
    00226
    00227   virtual void add(double *value, double noiseStrength) {
    00228     for (unsigned int i = 0; i < dimension; i++){
    00229       mean[i]+=sqrttau * WhiteNormalNoise::generate()*noiseStrength - tau*mean[i];
    00230       value[i]+=mean[i];
    00231     }
    00232   }
    00233
    00234   virtual double getTau(){ return tau;}
    00235   virtual void setTau(double newTau){
    00236     if(newTau >=0 && newTau <= 1){
    00237       tau=newTau;
    00238       sqrttau = sqrt(tau);
    00239     }
    00240   }
    00241
    00242 protected:
    00243   double tau; // smoothing paramter
    00244   double sqrttau; // square root of smoothing parameter
    00245   double* mean;
    00246   double mean1channel;
    00247   double factor;
    00248 };
    00249
    00250 /// Sine wave noise. Produces a 90 degree phase shifted sine wave or white noise
    00251 class SineWhiteNoise : public NoiseGenerator{
    00252 public:
    00253   /** @param omega anglerate
    00254       @param amplitude weighting of sine wave against noise strength
    00255       @param phaseShift phase shift between channels in rad
    00256       @param channels number of channel for sine noise (and the rest get white noise)
    00257    */
    00258   SineWhiteNoise(double omega, double amplitude, double phaseShift = M_PI/2,
    00259                  unsigned int channels = 0xFFFF)
    00260     : omega(omega), amplitude(amplitude)
    00261     , channels(channels), phaseShift(phaseShift){
    00262     t=0;
    00263   }
    00264
    00265   virtual ~SineWhiteNoise(){}
    00266
    00267   virtual double generate() {
    00268     t ++;
    00269     return (1-amplitude)*uniform(-1,  1) + sin(t*omega)*amplitude;
    00270   }
    00271
    00272   /** adds multidimensional noise to the value field.
    00273       @param value field where noise is added. Must have length dimension (\ref init())
    00274    */
    00275   virtual void add(double *value, double noiseStrength) { // min, double max){
    00276
    00277     for (unsigned int i = 0; i < dimension; i++){
    00278       if(i < channels){
    00279         value[i]+=sin(t*omega+i*phaseShift)*amplitude*noiseStrength;
    00280         value[i]+=(1-amplitude)*uniform(-1,  1)*noiseStrength;
    00281       }else{
    00282         value[i]+=uniform(-1,  1)*noiseStrength;
    00283       }
    00284     }
    00285     t ++;
    00286   }
    00287   void setOmega(double omega){
    00288     this->omega=omega;
    00289   }
    00290   void setPhaseShift(double phaseShift){
    00291     this->phaseShift=phaseShift;
    00292   }
    00293
    00294 protected:
    00295   long int t;        // time
    00296   double omega;     // angle velocity
    00297   double amplitude; // factor for noise strength
    00298   unsigned int channels;     // number of channels with sine
    00299   double phaseShift; // phase shift
    00300
    00301 };
    00302
    00303
    00304 #endif
    00305