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