ADMB Documentation  11.1.2397
 All Classes Files Functions Variables Typedefs Friends Defines
df1b2lp11.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id: df1b2lp11.cpp 2295 2014-09-04 23:35:11Z johnoel $
00003  *
00004  * Author: David Fournier
00005  * Copyright (c) 2008-2012 Regents of the University of California
00006  */
00011 #if defined(USE_DD)
00012 #  define USE_DD_STUFF
00013 #endif
00014 
00015 #  include <admodel.h>
00016 #  include <df1b2fun.h>
00017 #  include <adrndeff.h>
00018 double calculate_laplace_approximation(const dvector& x,const dvector& u0,
00019   const banded_symmetric_dmatrix& bHess,const dvector& _xadjoint,
00020   const dvector& _uadjoint,
00021   const banded_symmetric_dmatrix& _bHessadjoint,function_minimizer * pmin);
00022 double calculate_laplace_approximation(const dvector& x,const dvector& u0,
00023   const dmatrix& Hess,const dvector& _xadjoint,const dvector& _uadjoint,
00024   const dmatrix& _Hessadjoint,function_minimizer * pmin);
00025 
00026 #if defined(USE_DD_STUFF)
00027 #  if defined(_MSC_VER)
00028     extern "C" _export  void dd_newton_raphson(int n,double * v,double * diag,
00029       double * ldiag, double * yy);
00030 #  else
00031     extern "C" void dd_newton_raphson(int n,double * v,double * diag,
00032       double * ldiag, double * yy);
00033 #  endif
00034 #endif
00035 
00040 void positivize(const banded_symmetric_dmatrix& _m,double id)
00041 {
00042   ADUNCONST(banded_symmetric_dmatrix,m)
00043   int mmin=m.indexmin();
00044   int mmax=m.indexmax();
00045   for (int i=mmin;i<=mmax;i++)
00046   {
00047     m(i,i)+=id;
00048   }
00049 }
00050 
00055 class safe_choleski_solver
00056 {
00057 public:
00058   safe_choleski_solver(double id);
00059   int hadbad;
00060   int dirty;
00061   double id;
00062   double angle;
00063   dvector solve(const banded_symmetric_dmatrix& _m,const dvector&_v);
00064 };
00065 
00070 safe_choleski_solver::safe_choleski_solver(double _id)
00071 {
00072   id=_id;
00073   hadbad=0;
00074   dirty=0;
00075 }
00076 
00077 banded_lower_triangular_dmatrix quiet_choleski_decomp(
00078   const banded_symmetric_dmatrix& _M,const int& _ierr);
00079 /*
00080 banded_lower_triangular_dmatrix quiet_choleski_decomp(
00081   const banded_symmetric_dmatrix& _M,const int& _ierr)
00082 {
00083   int & ierr = (int &) _ierr;
00084   ADUNCONST(banded_symmetric_dmatrix,M)
00085   int minsave=M.indexmin();
00086   M.shift(1);
00087   int n=M.indexmax();
00088 
00089   int bw=M.bandwidth();
00090   banded_lower_triangular_dmatrix L(1,n,bw);
00091 #ifndef SAFE_INITIALIZE
00092     L.initialize();
00093 #endif
00094 
00095   int i,j,k;
00096   double tmp;
00097     if (M(1,1)<=0)
00098     {
00099       ierr=1;
00100       return L;
00101     }
00102   L(1,1)=sqrt(M(1,1));
00103   for (i=2;i<=bw;i++)
00104   {
00105     L(i,1)=M(i,1)/L(1,1);
00106   }
00107 
00108   for (i=2;i<=n;i++)
00109   {
00110     for (j=i-bw+1;j<=i-1;j++)
00111     {
00112       if (j>1)
00113       { 
00114         tmp=M(i,j);
00115         for (k=i-bw+1;k<=j-1;k++)
00116         {
00117     if (k>0 && k>j-bw)
00118             tmp-=L(i,k)*L(j,k);
00119         }
00120         L(i,j)=tmp/L(j,j);
00121       }
00122     }
00123     tmp=M(i,i);
00124     for (k=i-bw+1;k<=i-1;k++)
00125     {
00126       if (k>0)  
00127         tmp-=L(i,k)*L(i,k);
00128     }
00129     if (tmp<=0)
00130     {
00131       ierr=1;
00132       return L;
00133     }
00134     L(i,i)=sqrt(tmp);
00135   }
00136   M.shift(minsave);
00137   L.shift(minsave);
00138 
00139   return L;
00140 }
00141 */
00142 
00147 dvector safe_choleski_solver::solve
00148   (const banded_symmetric_dmatrix& _m,const dvector&_v)
00149 {
00150   int ierr=0;
00151   ADUNCONST(dvector,v)
00152   ADUNCONST(banded_symmetric_dmatrix,m)
00153   int mmin=m.indexmin();
00154   //int mmax=m.indexmax();
00155   if (hadbad && id>0.0)
00156   {
00157     positivize(m,id);
00158    dirty=1;
00159   }
00160   m.shift(1);
00161   v.shift(1);
00162   int ibreak=1;
00163   dvector w;
00164   do
00165   {
00166     const banded_lower_triangular_dmatrix& C=quiet_choleski_decomp(m,ierr);
00167     if (ierr==0)
00168     {
00169       id/=2.0;
00170       w=solve_trans(C,::solve(C,v));
00171       dvector delta=m*w;
00172       dvector err=solve_trans(C,::solve(C,v-delta));
00173       dvector w1=w+err;
00174       cout << norm(w1-w) << endl;
00175       if (norm(err)>1.e-10)
00176       {
00177         cout << "precisionerror" << endl;
00178       }
00179       angle=w*v/(norm(w)*norm(v));
00180       ibreak=0;
00181     }
00182     else
00183     {
00184       id*=2.0;
00185       positivize(m,id);
00186       ierr=0;
00187       dirty=1;
00188       hadbad=1;
00189     }
00190   }
00191   while(ibreak);
00192   m.shift(mmin);
00193   w.shift(mmin);
00194   v.shift(mmin);
00195   return w;
00196 }
00197 
00202 void laplace_approximation_calculator::
00203   do_newton_raphson_state_space(function_minimizer * pfmin,double f_from_1,
00204   int& no_converge_flag)
00205 {
00206   laplace_approximation_calculator::where_are_we_flag=2;
00207   double fbest=1.e+100;
00208   double fval=1.e+100;
00209   double maxg=fabs(evaluate_function(fbest,uhat,pfmin));
00210 
00211 
00212   laplace_approximation_calculator::where_are_we_flag=0;
00213   dvector uhat_old(1,usize);
00214   safe_choleski_solver scs(0.1);
00215   //for(int ii=1;ii<=num_nr_iters;ii++)
00216   int ii=0;
00217   do
00218   {
00219     bHess->initialize();
00220 
00221     grad.initialize();
00222 
00223     step=get_newton_raphson_info_banded(pfmin);
00224 
00225     // check for degenerate Hessian
00226     int check_hessian=0;
00227     if (check_hessian)
00228     {
00229       ofstream ofs("hh");
00230       ofs << colsum(dmatrix(*bHess)) << endl;
00231     }
00232 
00233     if (!initial_params::mc_phase)
00234       cout << "Newton raphson " << ii << "  ";
00235 
00236     if (quadratic_prior::get_num_quadratic_prior()>0)
00237     {
00238       quadratic_prior::get_cHessian_contribution(Hess,xsize);
00239       quadratic_prior::get_cgradient_contribution(grad,xsize);
00240     }
00241 
00242     //int ierr=0;
00243 
00244     dvector g1(1,usize);
00245     maxg=fabs(evaluate_function(fval,uhat,g1,pfmin));
00246     if (fval>fbest)
00247     fbest=fval;
00248     if (nr_debug==1)
00249     {
00250       cout << " grad compare " << norm(g1-grad)  << endl;
00251     }
00252     step=scs.solve(*bHess,g1);
00253     //step=scs.solve(*bHess,grad);
00254     if (nr_debug==1)
00255     {
00256       cout << " angle = " << step*grad/(norm(step)*norm(grad)) << endl;
00257     }
00258     int iic=0;
00259     double testangle=-1;
00260     int extra_try=0;
00261     dvector utry(1,usize);
00262     int smallshrink=0;
00263     do
00264     {
00265       if (++iic>10)
00266       {
00267         break;
00268       }
00269       if (extra_try==0)
00270       {
00271         utry = uhat-step;
00272       }
00273       else
00274       {
00275         utry = uhat-0.5*step;
00276       }
00277       dvector g(1,usize);
00278       maxg=fabs(evaluate_function(fval,utry,g,pfmin));
00279       if (nr_debug==1)
00280       {
00281         cout << "  fbest-fval = " << setprecision(15)
00282            <<  fbest-fval  << endl;
00283       }
00284       if (fval>fbest && maxg>1.e-10)
00285       {
00286         if (maxg<1.e-10)
00287           smallshrink=3;
00288         else if (maxg<1.e-9)
00289           smallshrink=2;
00290         else if (maxg<1.e-8)
00291           smallshrink=1;
00292 
00293         if (nr_debug==1)
00294         {
00295           testangle=g*step/(norm(g)*norm(step));
00296           cout << fval-fbest << " step too large  angle = " << testangle
00297                << endl;
00298         }
00299       }
00300       if (fval==fbest)
00301       {
00302         testangle=g*step/(norm(g)*norm(step));
00303         cout << " no progress  angle = " << testangle << endl;
00304       }
00305       if (fval<=fbest)
00306       {
00307         uhat=utry;
00308         fbest=fval;
00309         testangle=g*step/(norm(g)*norm(step));
00310         if (nr_debug==1)
00311         {
00312           cout << "inner angle = " << testangle << endl;
00313         }
00314         if (testangle>0.4)
00315         {
00316           extra_try=extra_try+1;
00317           if (nr_debug==1)
00318           {
00319             cout << extra_try << endl;
00320           }
00321         }
00322         else
00323         {
00324           break;
00325         }
00326       }
00327       else
00328       {
00329         if (extra_try>0)
00330         {
00331           break;
00332         }
00333         else
00334         {
00335           if (smallshrink==0)
00336             step/=100.0;
00337           else if(smallshrink==1)
00338             step/=10.0;
00339           else if(smallshrink==2)
00340             step/=5;
00341           else if(smallshrink==3)
00342             step/=2;
00343           smallshrink=0;
00344         }
00345       }
00346     }
00347     while(1);
00348 
00349     ii++;
00350 
00351     if (scs.dirty==1)
00352     {
00353       scs.dirty=0;
00354       step=get_newton_raphson_info_banded(pfmin);
00355       int print_hessian=0;
00356       if (print_hessian)
00357       {
00358         ofstream ofs("hh1");
00359         ofs << setw(12) << setscientific() << setprecision(3) << endl;
00360       }
00361 
00362       if (quadratic_prior::get_num_quadratic_prior()>0)
00363       {
00364         quadratic_prior::get_cHessian_contribution(Hess,xsize);
00365         quadratic_prior::get_cgradient_contribution(grad,xsize);
00366       }
00367       if (ii>=num_nr_iters || maxg < 1.e-13 )
00368       {
00369         step=scs.solve(*bHess,g1);
00370       }
00371       //solve(*bHess,grad);
00372     }
00373 
00374     for (int i=1;i<=usize;i++)
00375     {
00376       y(i+xsize)=uhat(i);
00377     }
00378 
00379     if (scs.dirty==0)
00380     {
00381       if (ii>=num_nr_iters || maxg < 1.e-13 )
00382       {
00383         break;
00384       }
00385     }
00386     else
00387     {
00388       scs.dirty=0;
00389       scs.hadbad=0;
00390       if (ii>100)
00391       {
00392         cerr << "Can't get positive definite Hessian in inner optimization"
00393              << endl;
00394         break;
00395       }
00396     }
00397   }
00398   while(1);
00399 }