gaussian_noise.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 
00033 #include <acado/noise/gaussian_noise.hpp>
00034 
00035 #include <stdlib.h>
00036 #include <time.h>
00037 
00038 
00039 
00040 BEGIN_NAMESPACE_ACADO
00041 
00042 
00043 
00044 
00045 GaussianNoise::GaussianNoise( ) : Noise( )
00046 {
00047 }
00048 
00049 
00050 GaussianNoise::GaussianNoise(   const DVector& _mean,
00051                                                                 const DVector& _variance
00052                                                                 ) : Noise( )
00053 {
00054         if ( _mean.getDim( ) == _variance.getDim( ) )
00055         {
00056                 w.init( _mean.getDim( ),1 );
00057                 mean = _mean;
00058                 variance = _variance;
00059         }
00060         else
00061                 ACADOERROR( RET_INVALID_NOISE_SETTINGS );
00062 }
00063 
00064 
00065 GaussianNoise::GaussianNoise(   uint _dim,
00066                                                                 double _mean,
00067                                                                 double _variance
00068                                                                 ) : Noise( )
00069 {
00070         w.init( _dim,1 );
00071 
00072         mean.init( _dim );
00073         variance.init( _dim );
00074         
00075         for( uint i=0; i<_dim; ++i )
00076         {
00077                 mean(i)     = _mean;
00078                 variance(i) = _variance;
00079         }
00080 }
00081 
00082 
00083 GaussianNoise::GaussianNoise( const GaussianNoise &rhs ) : Noise( rhs )
00084 {
00085         mean = rhs.mean;
00086         variance = rhs.variance;
00087 }
00088 
00089 
00090 GaussianNoise::~GaussianNoise( )
00091 {
00092 }
00093 
00094 
00095 GaussianNoise& GaussianNoise::operator=( const GaussianNoise &rhs )
00096 {
00097         if( this != &rhs )
00098         {
00099                 Noise::operator=( rhs );
00100 
00101                 mean = rhs.mean;
00102                 variance = rhs.variance;
00103         }
00104 
00105         return *this;
00106 }
00107 
00108 
00109 GaussianNoise* GaussianNoise::clone( ) const
00110 {
00111     return ( new GaussianNoise( *this ) );
00112 }
00113 
00114 
00115 GaussianNoise* GaussianNoise::clone(    uint idx
00116                                                                                 ) const
00117 {
00118         if ( idx >= getDim( ) )
00119                 return 0;
00120 
00121         GaussianNoise tmp( DVector(1),DVector(1) );
00122         tmp.Noise::operator=( *this );
00123         tmp.w.init( 1,1 );
00124         tmp.mean(0)     = mean(idx);
00125         tmp.variance(0) = variance(idx);
00126 
00127     return ( new GaussianNoise( tmp ) );
00128 }
00129 
00130 
00131 
00132 returnValue GaussianNoise::init(        uint seed
00133                                                                         )
00134 {
00135         if ( mean.getDim( ) != variance.getDim( ) )
00136                 return ACADOERROR( RET_INVALID_NOISE_SETTINGS );
00137 
00138         if ( mean.getDim( ) == 0 )
00139                 return ACADOERROR( RET_NO_NOISE_SETTINGS );
00140 
00141         /* initialize random seed: */
00142         if ( seed == 0 )
00143                 srand( (unsigned int)time(0) );
00144         else
00145                 srand( seed );
00146 
00147         setStatus( BS_READY );
00148 
00149         return SUCCESSFUL_RETURN;
00150 }
00151 
00152 
00153 returnValue GaussianNoise::step(        DVector& _w
00154                                                                         )
00155 {
00156         if ( getStatus( ) != BS_READY )
00157                 return ACADOERROR( RET_BLOCK_NOT_READY );
00158 
00159         if ( getDim( ) != _w.getDim( ) )
00160                 return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00161 
00162         if ( w.getNumPoints( ) != 1 )
00163                 w.init( 1,getDim() );
00164 
00165         for( uint j=0; j<getDim( ); ++j )
00166                 w(0,j) = getGaussianRandomNumber( mean(j),variance(j) );
00167 
00168         _w = w.getVector( 0 );
00169 
00170         return SUCCESSFUL_RETURN;
00171 }
00172 
00173 
00174 returnValue GaussianNoise::step(        VariablesGrid& _w
00175                                                                         )
00176 {
00177         if ( getStatus( ) != BS_READY )
00178                 return ACADOERROR( RET_BLOCK_NOT_READY );
00179 
00180         if ( getDim( ) != _w.getNumValues( ) )
00181                 return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00182 
00183         if ( w.getNumPoints( ) != _w.getNumPoints( ) )
00184                 w.init( getDim(),_w.getNumPoints( ) );
00185 
00186         for( uint i=0; i<_w.getNumPoints( ); ++i )
00187                 for( uint j=0; j<getDim( ); ++j )
00188                         w(i,j) = getGaussianRandomNumber( mean(j),variance(j) );
00189 
00190         _w = w;
00191 
00192         return SUCCESSFUL_RETURN;
00193 }
00194 
00195 
00196 //
00197 // PROTECTED MEMBER FUNCTIONS:
00198 //
00199 
00200 double GaussianNoise::getGaussianRandomNumber(  double _mean,
00201                                                                                                 double _variance
00202                                                                                                 ) const
00203 {
00204         // Box-Muller method
00205         double norm = 2.0;
00206         double uniformRandomNumber1, uniformRandomNumber2;
00207 
00208         while ( norm >= 1.0 )
00209         {
00210                 uniformRandomNumber1 = getUniformRandomNumber( -1.0,1.0 );
00211                 uniformRandomNumber2 = getUniformRandomNumber( -1.0,1.0 );
00212                 norm = uniformRandomNumber1*uniformRandomNumber1 + uniformRandomNumber2*uniformRandomNumber2;
00213         }
00214    
00215         double gaussianRandomNumber1 = sqrt( -2.0 * log(norm)/norm ) * uniformRandomNumber1;
00216         double gaussianRandomNumber2 = sqrt( -2.0 * log(norm)/norm ) * uniformRandomNumber2;
00217 
00218         return _mean + sqrt( _variance ) * (gaussianRandomNumber1+gaussianRandomNumber2)/2.0;
00219 }
00220 
00221 
00222 
00223 
00224 CLOSE_NAMESPACE_ACADO
00225 
00226 // end of file.


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:37:10