ADMB Documentation  11.1.2490
 All Classes Files Functions Variables Typedefs Friends Defines
df1b2lp11.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id: df1b2lp11.cpp 2477 2014-10-13 21:51:27Z 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 
00201 void laplace_approximation_calculator::
00202   do_newton_raphson_state_space(function_minimizer * pfmin,double f_from_1,
00203   int& no_converge_flag)
00204 {
00205   laplace_approximation_calculator::where_are_we_flag=2;
00206   double fbest=1.e+100;
00207   double fval=1.e+100;
00208   double maxg=fabs(evaluate_function(fbest,uhat,pfmin));
00209 
00210   laplace_approximation_calculator::where_are_we_flag=0;
00211   dvector uhat_old(1,usize);
00212   safe_choleski_solver scs(0.1);
00213   //for(int ii=1;ii<=num_nr_iters;ii++)
00214   int ii=0;
00215   for (;;)
00216   {
00217     bHess->initialize();
00218 
00219     grad.initialize();
00220 
00221     step=get_newton_raphson_info_banded(pfmin);
00222 
00223     // check for degenerate Hessian
00224     int check_hessian=0;
00225     if (check_hessian)
00226     {
00227       ofstream ofs("hh");
00228       ofs << colsum(dmatrix(*bHess)) << endl;
00229     }
00230 
00231     if (!initial_params::mc_phase)
00232       cout << "Newton raphson " << ii << "  ";
00233 
00234     if (quadratic_prior::get_num_quadratic_prior()>0)
00235     {
00236       quadratic_prior::get_cHessian_contribution(Hess,xsize);
00237       quadratic_prior::get_cgradient_contribution(grad,xsize);
00238     }
00239 
00240     //int ierr=0;
00241 
00242     dvector g1(1,usize);
00243     maxg=fabs(evaluate_function(fval,uhat,g1,pfmin));
00244     if (fval>fbest)
00245     fbest=fval;
00246     if (nr_debug==1)
00247     {
00248       cout << " grad compare " << norm(g1-grad)  << endl;
00249     }
00250     step=scs.solve(*bHess,g1);
00251     //step=scs.solve(*bHess,grad);
00252     if (nr_debug==1)
00253     {
00254       cout << " angle = " << step*grad/(norm(step)*norm(grad)) << endl;
00255     }
00256     int iic=0;
00257     double testangle=-1;
00258     int extra_try=0;
00259     dvector utry(1,usize);
00260     int smallshrink=0;
00261     for (;;)
00262     {
00263       if (++iic>10)
00264       {
00265         break;
00266       }
00267       if (extra_try==0)
00268       {
00269         utry = uhat-step;
00270       }
00271       else
00272       {
00273         utry = uhat-0.5*step;
00274       }
00275       dvector g(1,usize);
00276       maxg=fabs(evaluate_function(fval,utry,g,pfmin));
00277       if (nr_debug==1)
00278       {
00279         cout << "  fbest-fval = " << setprecision(15)
00280            <<  fbest-fval  << endl;
00281       }
00282       if (fval>fbest && maxg>1.e-10)
00283       {
00284         if (maxg<1.e-10)
00285           smallshrink=3;
00286         else if (maxg<1.e-9)
00287           smallshrink=2;
00288         else if (maxg<1.e-8)
00289           smallshrink=1;
00290 
00291         if (nr_debug==1)
00292         {
00293           testangle=g*step/(norm(g)*norm(step));
00294           cout << fval-fbest << " step too large  angle = " << testangle
00295                << endl;
00296         }
00297       }
00298       if (fval==fbest)
00299       {
00300         testangle=g*step/(norm(g)*norm(step));
00301         cout << " no progress  angle = " << testangle << endl;
00302       }
00303       if (fval<=fbest)
00304       {
00305         uhat=utry;
00306         fbest=fval;
00307         testangle=g*step/(norm(g)*norm(step));
00308         if (nr_debug==1)
00309         {
00310           cout << "inner angle = " << testangle << endl;
00311         }
00312         if (testangle>0.4)
00313         {
00314           extra_try=extra_try+1;
00315           if (nr_debug==1)
00316           {
00317             cout << extra_try << endl;
00318           }
00319         }
00320         else
00321         {
00322           break;
00323         }
00324       }
00325       else
00326       {
00327         if (extra_try>0)
00328         {
00329           break;
00330         }
00331         else
00332         {
00333           if (smallshrink==0)
00334             step/=100.0;
00335           else if(smallshrink==1)
00336             step/=10.0;
00337           else if(smallshrink==2)
00338             step/=5;
00339           else if(smallshrink==3)
00340             step/=2;
00341           smallshrink=0;
00342         }
00343       }
00344     }
00345 
00346     ii++;
00347 
00348     if (scs.dirty==1)
00349     {
00350       scs.dirty=0;
00351       step=get_newton_raphson_info_banded(pfmin);
00352       int print_hessian=0;
00353       if (print_hessian)
00354       {
00355         ofstream ofs("hh1");
00356         ofs << setw(12) << setscientific() << setprecision(3) << endl;
00357       }
00358 
00359       if (quadratic_prior::get_num_quadratic_prior()>0)
00360       {
00361         quadratic_prior::get_cHessian_contribution(Hess,xsize);
00362         quadratic_prior::get_cgradient_contribution(grad,xsize);
00363       }
00364       if (ii>=num_nr_iters || maxg < 1.e-13 )
00365       {
00366         step=scs.solve(*bHess,g1);
00367       }
00368       //solve(*bHess,grad);
00369     }
00370 
00371     for (int i=1;i<=usize;i++)
00372     {
00373       y(i+xsize)=uhat(i);
00374     }
00375 
00376     if (scs.dirty==0)
00377     {
00378       if (ii>=num_nr_iters || maxg < 1.e-13 )
00379       {
00380         break;
00381       }
00382     }
00383     else
00384     {
00385       scs.dirty=0;
00386       scs.hadbad=0;
00387       if (ii>100)
00388       {
00389         cerr << "Can't get positive definite Hessian in inner optimization"
00390              << endl;
00391         break;
00392       }
00393     }
00394   }
00395 }