sensor.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 
00034 #include <acado/transfer_device/sensor.hpp>
00035 
00036 
00037 
00038 BEGIN_NAMESPACE_ACADO
00039 
00040 
00041 
00042 //
00043 // PUBLIC MEMBER FUNCTIONS:
00044 //
00045 
00046 Sensor::Sensor( ) : TransferDevice( BN_SENSOR )
00047 {
00048 }
00049 
00050 
00051 Sensor::Sensor( uint _nY,
00052                                 double _samplingTime
00053                                 ) : TransferDevice( _nY,BN_SENSOR,_samplingTime )
00054 {
00055 }
00056 
00057 
00058 Sensor::Sensor( const Sensor& rhs ) : TransferDevice( rhs )
00059 {
00060 }
00061 
00062 
00063 Sensor::~Sensor( )
00064 {
00065 }
00066 
00067 
00068 Sensor& Sensor::operator=( const Sensor& rhs )
00069 {
00070     if ( this != &rhs )
00071     {
00072                 TransferDevice::operator=( rhs );
00073 
00074                 
00075     }
00076 
00077     return *this;
00078 }
00079 
00080 
00081 returnValue Sensor::setOutputNoise(     const Noise& _noise,
00082                                                                         double _noiseSamplingTime
00083                                                                         )
00084 {
00085         if ( _noise.getDim( ) != getNY( ) )
00086                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00087 
00088         for( uint i=0; i<getNY( ); ++i )
00089         {
00090                 if ( additiveNoise[i] != 0 )
00091                         delete additiveNoise[i];
00092 
00093                 additiveNoise[i] = _noise.clone( i );
00094         }
00095 
00096         noiseSamplingTimes.setAll( _noiseSamplingTime );
00097 
00098         return SUCCESSFUL_RETURN;
00099 }
00100 
00101 
00102 returnValue Sensor::setOutputNoise(     uint idx,
00103                                                                         const Noise& _noise,
00104                                                                         double _noiseSamplingTime
00105                                                                         )
00106 {
00107         if ( ( idx >= getNY( ) ) || ( _noise.getDim( ) != 1 ) )
00108                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00109 
00110         if ( additiveNoise[idx] != 0 )
00111                 delete additiveNoise[idx];
00112 
00113         additiveNoise[idx] = _noise.clone( );
00114 
00115         if ( ( idx > 0 ) && ( acadoIsEqual( _noiseSamplingTime, noiseSamplingTimes(0) ) == BT_FALSE ) )
00116                 ACADOWARNING( RET_NO_DIFFERENT_NOISE_SAMPLING_FOR_DISCRETE );
00117 
00118         noiseSamplingTimes.setAll( _noiseSamplingTime ); // should be changed later
00119 
00120         return SUCCESSFUL_RETURN;
00121 }
00122 
00123 
00124 
00125 returnValue Sensor::setOutputDeadTimes( const DVector& _deadTimes
00126                                                                                 )
00127 {
00128         if ( _deadTimes.getDim( ) != getNY( ) )
00129                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00130 
00131         if ( _deadTimes.getMin( ) < 0.0 )
00132                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00133 
00134         if ( deadTimes.getDim( ) == 0 )
00135                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
00136 
00137         for( uint i=0; i<getNY(); ++i )
00138                 deadTimes( i ) = _deadTimes( i );
00139 
00140         return SUCCESSFUL_RETURN;
00141 }
00142 
00143 
00144 returnValue Sensor::setOutputDeadTimes( double _deadTime
00145                                                                                 )
00146 {
00147         if ( _deadTime < 0.0 )
00148                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00149 
00150         if ( deadTimes.getDim( ) == 0 )
00151                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
00152 
00153         for( uint i=0; i<getNY(); ++i )
00154                 deadTimes( i ) = _deadTime;
00155 
00156         return SUCCESSFUL_RETURN;
00157 }
00158 
00159 
00160 returnValue Sensor::setOutputDeadTime(  uint idx,
00161                                                                                 double _deadTime
00162                                                                                 )
00163 {
00164         if ( idx >= getNY( ) )
00165                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00166 
00167         if ( _deadTime < 0.0 )
00168                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00169 
00170         if ( deadTimes.getDim( ) == 0 )
00171                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
00172 
00173         deadTimes( idx ) = _deadTime;
00174 
00175         return SUCCESSFUL_RETURN;
00176 }
00177 
00178 
00179 
00180 returnValue Sensor::init(       double _startTime,
00181                                                         const DVector& _startValue
00182                                                         )
00183 {
00184         if ( TransferDevice::init( _startTime,_startValue ) != SUCCESSFUL_RETURN )
00185                 return ACADOERROR( RET_SENSOR_INIT_FAILED );
00186 
00187         return SUCCESSFUL_RETURN;
00188 }
00189 
00190 
00191 
00192 returnValue Sensor::step(       VariablesGrid& _y
00193                                                         )
00194 {
00195         // consistency checks
00196         if ( getStatus( ) != BS_READY )
00197                 return ACADOERROR( RET_BLOCK_NOT_READY );
00198 
00199         if ( ( _y.getNumPoints( ) < 2 ) || ( _y.getNumRows( ) != getNY( ) ) )
00200                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00201 
00202         if ( acadoIsEqual( _y.getFirstTime( ),lastSignal.getLastTime( ) ) == BT_FALSE )
00203                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00204 
00205 
00206         // delay inputs and store last signal
00207         if ( delaySensorOutput( _y ) != SUCCESSFUL_RETURN )
00208                 return ACADOERROR( RET_SENSOR_STEP_FAILED );
00209         
00210         // add sensor noise
00211         if ( addSensorNoise( _y ) != SUCCESSFUL_RETURN )
00212                 return ACADOERROR( RET_SENSOR_STEP_FAILED );
00213 
00214         return SUCCESSFUL_RETURN;
00215 }
00216 
00217 
00218 
00219 //
00220 // PROTECTED MEMBER FUNCTIONS:
00221 //
00222 
00223 
00224 returnValue Sensor::delaySensorOutput(  VariablesGrid& _y
00225                                                                                 )
00226 {
00227         if ( hasDeadTime( ) == BT_FALSE )
00228         {
00229                 // store last signal
00230                 DVector tmp = _y.getLastVector( );
00231 
00232                 lastSignal.init( tmp );
00233                 lastSignal.setTime( 0,_y.getLastTime( ) );
00234         
00235                 return SUCCESSFUL_RETURN;
00236         }
00237         else
00238         {
00239                 double startTime = _y.getFirstTime( );
00240                 double endTime   = _y.getLastTime( );
00241 
00242                 // determine variables grid of delayed input
00243                 VariablesGrid yDelayed;
00244                 if ( getDelayedOutputGrid( _y, yDelayed ) != SUCCESSFUL_RETURN )
00245                         return ACADOERROR( RET_DELAYING_OUTPUTS_FAILED );
00246 
00247                 // store last signal
00248                 lastSignal = yDelayed.getTimeSubGrid( yDelayed.getFloorIndex( endTime ),yDelayed.getLastIndex( ) );
00249 
00250                 // crop delayed signal to current horizon
00251                 _y = yDelayed.getTimeSubGrid( yDelayed.getFloorIndex( startTime ),yDelayed.getFloorIndex( endTime ) );
00252 
00253                 return SUCCESSFUL_RETURN;
00254         }
00255 }
00256 
00257 
00258 returnValue Sensor::getDelayedOutputGrid(       const VariablesGrid& _y,
00259                                                                                         VariablesGrid& _yDelayed
00260                                                                                         ) const
00261 {
00262         // determine common time grid for delayed outputs:
00263         Grid delayedOutputTimeGrid = lastSignal.getTimePoints( );
00264 
00265         // make sure that last time instant of horizon lies within the grid
00266         if ( acadoIsEqual( lastSignal.getLastTime(),_y.getLastTime( ) ) == BT_FALSE )
00267                 delayedOutputTimeGrid.addTime( _y.getLastTime( ) );
00268 
00269         // add grids of all delayed output components
00270         for( uint i=0; i<getNY( ); ++i )
00271                 delayedOutputTimeGrid.merge( _y.getTimePoints( ).shiftTimes( deadTimes(i) ),MM_REPLACE );
00272 
00273         VariablesGrid tmp;
00274 
00275         // setup common variables grid for delayed inputs
00276         _yDelayed.init( );
00277 
00278         for( uint i=0; i<getNY( ); ++i )
00279         {
00280                 tmp = lastSignal( i );
00281                 tmp.merge( _y( i ).shiftTimes( deadTimes(i) ),MM_REPLACE,BT_FALSE );
00282                 tmp.refineGrid( delayedOutputTimeGrid );
00283 
00284                 _yDelayed.appendValues( tmp );
00285         }
00286 
00287         return SUCCESSFUL_RETURN;
00288 }
00289 
00290 
00291 
00292 returnValue Sensor::addSensorNoise(     VariablesGrid& _y
00293                                                                         ) const
00294 {
00295         if ( hasNoise( ) == BT_FALSE )
00296                 return SUCCESSFUL_RETURN;
00297 
00298         // generate current noise
00299         VariablesGrid currentNoise;
00300 
00301         if ( generateNoise( _y.getFirstTime(),_y.getLastTime(),currentNoise ) != SUCCESSFUL_RETURN )
00302                 return ACADOERROR( RET_GENERATING_NOISE_FAILED );
00303 
00304         // determine common grid
00305         Grid commonGrid, tmpGrid;
00306         _y.getGrid( commonGrid );
00307         currentNoise.getGrid( tmpGrid );
00308         commonGrid.merge( tmpGrid,MM_KEEP );
00309         
00310         // adapt input grids and add noise
00311         _y.refineGrid( commonGrid );
00312         currentNoise.refineGrid( commonGrid );
00313         _y += currentNoise.getValuesSubGrid( 0,getNY()-1 );
00314 
00315         return SUCCESSFUL_RETURN;
00316 }
00317 
00318 
00319 
00320 CLOSE_NAMESPACE_ACADO
00321 
00322 /*
00323  *      end of file
00324  */


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