bfgs_update.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/nlp_derivative_approximation/bfgs_update.hpp>
00035 
00036 
00037 
00038 BEGIN_NAMESPACE_ACADO
00039 
00040 
00041 //
00042 // PUBLIC MEMBER FUNCTIONS:
00043 //
00044 
00045 BFGSupdate::BFGSupdate( ) : ConstantHessian( )
00046 {
00047         modification = MOD_POWELLS_MODIFICATION;
00048         nBlocks = 0;
00049 }
00050 
00051 
00052 BFGSupdate::BFGSupdate( UserInteraction* _userInteraction,
00053                                                 uint _nBlocks
00054                                                 ) : ConstantHessian( _userInteraction )
00055 {
00056         modification = MOD_POWELLS_MODIFICATION;
00057         nBlocks = _nBlocks;
00058 }
00059 
00060 
00061 BFGSupdate::BFGSupdate( const BFGSupdate& rhs ) : ConstantHessian( rhs )
00062 {
00063     modification = rhs.modification;
00064         nBlocks = rhs.nBlocks;
00065 }
00066 
00067 
00068 BFGSupdate::~BFGSupdate( )
00069 {
00070 }
00071 
00072 
00073 BFGSupdate& BFGSupdate::operator=( const BFGSupdate& rhs )
00074 {
00075         if ( this != &rhs )
00076         {
00077                 ConstantHessian::operator=( rhs );
00078 
00079                 modification = rhs.modification;
00080                 nBlocks = rhs.nBlocks;
00081         }
00082 
00083         return *this;
00084 }
00085 
00086 
00087 NLPderivativeApproximation* BFGSupdate::clone( ) const
00088 {
00089         return new BFGSupdate( *this );
00090 }
00091 
00092 
00093 
00094 returnValue BFGSupdate::initHessian(    BlockMatrix& B,
00095                                                                                 uint N,
00096                                                                                 const OCPiterate& iter
00097                                                                                 )
00098 {
00099         return ConstantHessian::initHessian( B,N,iter );
00100 }
00101 
00102 
00103 returnValue BFGSupdate::initScaling(    BlockMatrix& B,
00104                                                                                 const BlockMatrix& x,
00105                                                                                 const BlockMatrix& y
00106                                                                                 )
00107 {
00108     return ConstantHessian::initScaling( B,x,y );
00109 }
00110 
00111 
00112 
00113 returnValue BFGSupdate::apply(  BlockMatrix &B,
00114                                                                 const BlockMatrix &x,
00115                                                                 const BlockMatrix &y
00116                                                                 )
00117 {
00118         if ( performsBlockUpdates( ) == BT_TRUE )
00119         return applyBlockDiagonalUpdate( B,x,y );
00120         else
00121         return applyUpdate( B,x,y );
00122 }
00123 
00124 
00125 
00126 //
00127 // PROTECTED MEMBER FUNCTIONS:
00128 //
00129 
00130 returnValue BFGSupdate::applyUpdate(    BlockMatrix &B,
00131                                                                                 const BlockMatrix &x,
00132                                                                                 const BlockMatrix &y
00133                                                                                 )
00134 {
00135     // CONSTANTS FOR POWELL'S STRATEGY:
00136     // --------------------------------
00137     const double epsilon = 0.2;   // constant epsilon for a positive curvature
00138                                   // check of the form x^T y > epsilon x^T B x
00139     double       theta   = 1.0;   // constant theta for Powell's strategy.
00140 
00141 
00142     // OTHER CONSTANTS:
00143     // ----------------
00144     const double regularisation = 100.0*EPS; // safe-guard constant for devisions
00145                                              // (to avoid numerical devision by 0).
00146 
00147 
00148     // DEFINITTIONS:
00149     // --------------             -------------------------------------------------------
00150     BlockMatrix Bx  ;             // the vector  B*x.
00151     BlockMatrix BxxB;             // the matrix  B*x*x^T*B
00152     DMatrix      xBx ;             // the scalar  x^T B x
00153     DMatrix      xy  ;             // the scalar  x^T y
00154     BlockMatrix z   ;             // the modified "gradient" y (needed for Powell's strategy)
00155     double      xz = 0.0;         // the scalar  x^T z
00156     BlockMatrix zz  ;             // the matrix  z*z^T
00157 
00158 
00159     // COMPUTATION OF Bx, xBx, xy AND BxxB:
00160     // -------------------------------------
00161     Bx = B*x;
00162     (x^Bx).getSubBlock( 0, 0, xBx, 1, 1 );
00163     (y^x ).getSubBlock( 0, 0, xy , 1, 1 );
00164     BxxB = Bx*Bx.transpose();
00165 
00166 
00167     // CURVATURE CHECK:
00168     // -------------------------------------
00169     if( xy(0,0) > epsilon*xBx(0,0) ){
00170 
00171         z  = y      ; // do not modify z if  x^T y > epsilon x^T B x
00172         xz = xy(0,0); // in this case we have x^T z = x^T y.
00173     }
00174     else{
00175 
00176         switch( modification ){
00177 
00178             case MOD_NO_MODIFICATION : // In this case no modification is applied
00179                                        // and B might become indefinite after the update
00180                  z  = y      ;
00181                  xz = xy(0,0);
00182                  break;
00183 
00184 
00185             case MOD_NOCEDALS_MODIFICATION:  // just skip the update
00186 
00187                  return SUCCESSFUL_RETURN;
00188 
00189 
00190             case MOD_POWELLS_MODIFICATION:  // apply Powell's modification of y
00191 
00192                  theta =      (1.0-epsilon)*xBx(0,0)/
00193                          (xBx(0,0)-xy(0,0)+regularisation);
00194 
00195                  z   = y               ;
00196                  z  *= theta           ;
00197                  Bx *= (1.0-theta)     ;
00198                  z  += Bx              ;
00199                  xz  = epsilon*xBx(0,0);
00200                  break;
00201         }
00202     }
00203 
00204     zz = z*z.transpose();
00205 
00206     BxxB *= 1.0/(xBx(0,0) + regularisation);
00207     zz   *= 1.0/(xz       + regularisation);
00208 
00209 
00210     // PERFORM THE UPDATE:
00211     // -------------------
00212 
00213     B += zz - BxxB;
00214 
00215     return SUCCESSFUL_RETURN;
00216 }
00217 
00218 
00219 returnValue BFGSupdate::applyBlockDiagonalUpdate(       BlockMatrix &B,
00220                                                                                                         const BlockMatrix &x,
00221                                                                                                         const BlockMatrix &y
00222                                                                                                         )
00223 {
00224     int run1;
00225     BlockMatrix a(5,1), b(5,1);
00226     BlockMatrix block(5,5);
00227 
00228     for( run1 = 0; run1 < (int)nBlocks; run1++ ){
00229 
00230         a.setZero();
00231         b.setZero();
00232         block.setZero();
00233 
00234         getSubBlockLine( nBlocks, 0, 0, run1, x, a );
00235         getSubBlockLine( nBlocks, 0, 0, run1, y, b );
00236 
00237         getSubBlockLine( nBlocks,           run1, 0, run1, B, block );
00238         getSubBlockLine( nBlocks,   nBlocks+run1, 1, run1, B, block );
00239         getSubBlockLine( nBlocks, 2*nBlocks+run1, 2, run1, B, block );
00240         getSubBlockLine( nBlocks, 3*nBlocks+run1, 3, run1, B, block );
00241         getSubBlockLine( nBlocks, 4*nBlocks+run1, 4, run1, B, block );
00242 
00243         applyUpdate( block, a, b );
00244 
00245         setSubBlockLine( nBlocks,           run1, 0, run1, B, block );
00246         setSubBlockLine( nBlocks,   nBlocks+run1, 1, run1, B, block );
00247         setSubBlockLine( nBlocks, 2*nBlocks+run1, 2, run1, B, block );
00248         setSubBlockLine( nBlocks, 3*nBlocks+run1, 3, run1, B, block );
00249         setSubBlockLine( nBlocks, 4*nBlocks+run1, 4, run1, B, block );
00250     }
00251 
00252     return SUCCESSFUL_RETURN;
00253 }
00254 
00255 
00256 
00257 returnValue BFGSupdate::getSubBlockLine( const int         &N     ,
00258                                          const int         &line1 ,
00259                                          const int         &line2 ,
00260                                          const int         &offset,
00261                                          const BlockMatrix &M     ,
00262                                                BlockMatrix &O      )
00263 {
00264         DMatrix tmp;
00265 
00266         M.getSubBlock(     offset, line1, tmp );
00267         if( tmp.getDim() != 0 ) O.setDense(0,line2,tmp);
00268 
00269         M.getSubBlock(   N+offset, line1, tmp );
00270         if( tmp.getDim() != 0 ) O.setDense(1,line2,tmp);
00271 
00272         M.getSubBlock( 2*N+offset, line1, tmp );
00273         if( tmp.getDim() != 0 ) O.setDense(2,line2,tmp);
00274 
00275         M.getSubBlock( 3*N+offset, line1, tmp );
00276         if( tmp.getDim() != 0 ) O.setDense(3,line2,tmp);
00277 
00278         M.getSubBlock( 4*N+offset, line1, tmp );
00279         if( tmp.getDim() != 0 ) O.setDense(4,line2,tmp);
00280 
00281         return SUCCESSFUL_RETURN;
00282 }
00283 
00284 
00285 returnValue BFGSupdate::setSubBlockLine( const int         &N     ,
00286                                          const int         &line1 ,
00287                                          const int         &line2 ,
00288                                          const int         &offset,
00289                                                BlockMatrix &M     ,
00290                                          const BlockMatrix &O      )
00291 {
00292         DMatrix tmp;
00293 
00294         O.getSubBlock( 0, line2, tmp );
00295 
00296         if( tmp.getDim() != 0 ) M.setDense(     offset, line1, tmp );
00297         O.getSubBlock( 1, line2, tmp );
00298         if( tmp.getDim() != 0 ) M.setDense(   N+offset, line1, tmp );
00299 
00300         O.getSubBlock(2,line2,tmp);
00301         if( tmp.getDim() != 0 ) M.setDense( 2*N+offset, line1, tmp );
00302 
00303         O.getSubBlock(3,line2,tmp);
00304         if( tmp.getDim() != 0 ) M.setDense( 3*N+offset, line1, tmp );
00305 
00306         O.getSubBlock(4,line2,tmp);
00307         if( tmp.getDim() != 0 ) M.setDense( 4*N+offset, line1, tmp );
00308 
00309         return SUCCESSFUL_RETURN;
00310 }
00311 
00312 
00313 
00314 
00315 
00316 //
00317 // PROTECTED MEMBER FUNCTIONS:
00318 //
00319 
00320 
00321 
00322 
00323 
00324 CLOSE_NAMESPACE_ACADO
00325 
00326 // end of file.


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