lsq_end_term.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 
00027 
00035 #include <acado/objective/lsq_end_term.hpp>
00036 
00037 
00038 BEGIN_NAMESPACE_ACADO
00039 
00040 
00041 
00042 //
00043 // PUBLIC MEMBER FUNCTIONS:
00044 //
00045 
00046 //
00047 // PUBLIC MEMBER FUNCTIONS:
00048 //
00049 
00050 
00051 LSQEndTerm::LSQEndTerm( )
00052            :ObjectiveElement(){
00053 
00054     S_h_res = 0;
00055 }
00056 
00057 
00058 LSQEndTerm::LSQEndTerm( const Grid& grid_, const DMatrix &S_,
00059                         const Function& m, const DVector &r_ )
00060            :ObjectiveElement( grid_ ){
00061 
00062     fcn  = m ;
00063     S    = S_;
00064     r    = r_;
00065 
00066     S_h_res = new double[fcn.getDim()];
00067 }
00068 
00069 
00070 LSQEndTerm::LSQEndTerm( const LSQEndTerm& rhs )
00071            :ObjectiveElement( rhs ){
00072 
00073     S = rhs.S;
00074     r = rhs.r;
00075 
00076     S_h_res = new double[fcn.getDim()];
00077 }
00078 
00079 
00080 LSQEndTerm::~LSQEndTerm( ){
00081 
00082     if( S_h_res != 0 ) delete[] S_h_res;
00083 }
00084 
00085 
00086 LSQEndTerm& LSQEndTerm::operator=( const LSQEndTerm& rhs ){
00087 
00088     if ( this != &rhs ){
00089 
00090         if( S_h_res != 0 ) delete[] S_h_res;
00091 
00092         ObjectiveElement::operator=(rhs);
00093 
00094         S = rhs.S;
00095         r = rhs.r;
00096 
00097         S_h_res = new double[fcn.getDim()];
00098     }
00099     return *this;
00100 }
00101 
00102 
00103 returnValue LSQEndTerm::evaluate( const OCPiterate &x ){
00104 
00105     uint run2, run3;
00106     const uint nh = fcn.getDim();
00107 
00108     ObjectiveElement::init( x );
00109 
00110     z.setZ( grid.getLastIndex(), x );
00111 
00112     DVector h_res = fcn.evaluate( z);
00113 
00114     // EVALUATE THE OBJECTIVE:
00115     // -----------------------
00116 
00117     obj = 0.0;
00118 
00119     for( run2 = 0; run2 < nh; run2++ )
00120         h_res(run2) -= r.operator()(run2);
00121 
00122     for( run2 = 0; run2 < nh; run2++ ){
00123         S_h_res[run2] = 0.0;
00124         for( run3 = 0; run3 < nh; run3++ )
00125             S_h_res[run2] += S.operator()(run2,run3)*h_res(run3);
00126     }
00127 
00128     for( run2 = 0; run2 < nh; run2++ )
00129         obj += 0.5*h_res(run2)*S_h_res[run2];
00130 
00131 
00132     return SUCCESSFUL_RETURN;
00133 }
00134 
00135 
00136 returnValue LSQEndTerm::evaluateSensitivities( BlockMatrix *hessian ){
00137 
00138     if( hessian == 0 ) return evaluateSensitivitiesGN(0);
00139     return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
00140 }
00141 
00142 
00143 returnValue LSQEndTerm::evaluateSensitivitiesGN( BlockMatrix *GNhessian ){
00144 
00145     int run2, run3, run4;
00146     const int N = grid.getNumPoints();
00147     const int nh = fcn.getDim();
00148 
00149     if( bSeed != 0 ){
00150 
00151         if( xSeed  != 0 || pSeed  != 0 || uSeed  != 0 || wSeed  != 0 ||
00152             xSeed2 != 0 || pSeed2 != 0 || uSeed2 != 0 || wSeed2 != 0 )
00153             return ACADOERROR( RET_WRONG_DEFINITION_OF_SEEDS );
00154 
00155         double *bseed   = new double [nh];
00156         double **J      = new double*[nh];
00157 
00158         for( run2 = 0; run2 < nh; run2++ )
00159              J[run2] = new double[fcn.getNumberOfVariables() +1];
00160 
00161         if( bSeed->getNumRows( 0, 0 ) != 1 ) return ACADOWARNING( RET_WRONG_DEFINITION_OF_SEEDS );
00162 
00163         DMatrix bseed_;
00164         bSeed->getSubBlock( 0, 0, bseed_);
00165 
00166         dBackward.init( 1, 5*N );
00167 
00168         DMatrix Dx ( 1, nx );
00169         DMatrix Dxa( 1, na );
00170         DMatrix Dp ( 1, np );
00171         DMatrix Du ( 1, nu );
00172         DMatrix Dw ( 1, nw );
00173 
00174         Dx .setZero();
00175         Dxa.setZero();
00176         Dp .setZero();
00177         Du .setZero();
00178         Dw .setZero();
00179 
00180         for( run2 = 0; run2 < nh; run2++ ) bseed[run2] = 0;
00181 
00182         for( run2 = 0; run2 < nh; run2++ ){
00183              for(run3 = 0; (int) run3 < fcn.getNumberOfVariables() +1; run3++ )
00184                  J[run2][run3] = 0.0;
00185 
00186              bseed[run2] = 1.0;
00187              fcn.AD_backward( 0, bseed, J[run2] );
00188              bseed[run2] = 0.0;
00189 
00190              for( run3 = 0; run3 < nx; run3++ ){
00191                   Dx( 0, run3 ) += bseed_(0,0)*J[run2][y_index[run3]]*S_h_res[run2];
00192              }
00193              for( run3 = nx; run3 < nx+na; run3++ ){
00194                   Dxa( 0, run3-nx ) += bseed_(0,0)*J[run2][y_index[run3]]*S_h_res[run2];
00195              }
00196              for( run3 = nx+na; run3 < nx+na+np; run3++ ){
00197                   Dp( 0, run3-nx-na ) += bseed_(0,0)*J[run2][y_index[run3]]*S_h_res[run2];
00198              }
00199              for( run3 = nx+na+np; run3 < nx+na+np+nu; run3++ ){
00200                   Du( 0, run3-nx-na-np ) += bseed_(0,0)*J[run2][y_index[run3]]*S_h_res[run2];
00201              }
00202              for( run3 = nx+na+np+nu; run3 < nx+na+np+nu+nw; run3++ ){
00203                   Dw( 0, run3-nx-na-np-nu ) += bseed_(0,0)*J[run2][y_index[run3]]*S_h_res[run2];
00204              }
00205         }
00206         if( nx > 0 ) dBackward.setDense( 0,   N-1, Dx  );
00207         if( na > 0 ) dBackward.setDense( 0, 2*N-1, Dxa );
00208         if( np > 0 ) dBackward.setDense( 0, 3*N-1, Dp  );
00209         if( nu > 0 ) dBackward.setDense( 0, 4*N-1, Du  );
00210         if( nw > 0 ) dBackward.setDense( 0, 5*N-1, Dw  );
00211 
00212 
00213         // COMPUTE GAUSS-NEWTON HESSIAN APPROXIMATION IF REQUESTED:
00214         // --------------------------------------------------------
00215 
00216         if( GNhessian != 0 ){
00217 
00218             const int nnn = nx+na+np+nu+nw;
00219             DMatrix tmp( nh, nnn );
00220 
00221             for( run3 = 0; run3 < nnn; run3++ ){
00222                 for( run2 = 0; run2 < nh; run2++ ){
00223                     tmp( run2, run3 ) = 0.0;
00224                     for( run4 = 0; run4 < nh; run4++ ){
00225                         tmp( run2, run3 ) += S.operator()(run2,run4)*J[run4][y_index[run3]];
00226                     }
00227                 }
00228             }
00229             DMatrix tmp2;
00230             int i,j;
00231             int *Sidx = new int[6];
00232             int *Hidx = new int[5];
00233 
00234             Sidx[0] = 0;
00235             Sidx[1] = nx;
00236             Sidx[2] = nx+na;
00237             Sidx[3] = nx+na+np;
00238             Sidx[4] = nx+na+np+nu;
00239             Sidx[5] = nx+na+np+nu+nw;
00240 
00241             Hidx[0] =   N-1;
00242             Hidx[1] = 2*N-1;
00243             Hidx[2] = 3*N-1;
00244             Hidx[3] = 4*N-1;
00245             Hidx[4] = 5*N-1;
00246 
00247             for( i = 0; i < 5; i++ ){
00248                 for( j = 0; j < 5; j++ ){
00249 
00250                     tmp2.init(Sidx[i+1]-Sidx[i],Sidx[j+1]-Sidx[j]);
00251                     tmp2.setZero();
00252 
00253                     for( run3 = Sidx[i]; run3 < Sidx[i+1]; run3++ )
00254                         for( run4 = Sidx[j]; run4 < Sidx[j+1]; run4++ )
00255                             for( run2 = 0; run2 < nh; run2++ )
00256                                 tmp2(run3-Sidx[i],run4-Sidx[j]) += J[run2][y_index[run3]]*tmp(run2,run4);
00257 
00258                     if( tmp2.getDim() != 0 ) GNhessian->addDense(Hidx[i],Hidx[j],tmp2);
00259                 }
00260             }
00261             delete[] Sidx;
00262             delete[] Hidx;
00263         }
00264         // --------------------------------------------------------
00265 
00266         for( run2 = 0; run2 < nh; run2++ )
00267             delete[] J[run2];
00268         delete[] J;
00269         delete[] bseed;
00270         return SUCCESSFUL_RETURN;
00271     }
00272 
00273     return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
00274 }
00275 
00276 
00277 
00278 
00279 CLOSE_NAMESPACE_ACADO
00280 
00281 // end of file.


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