ADMB Documentation  11.1.2274
 All Classes Files Functions Variables Typedefs Friends Defines
df1b2lp11.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id: df1b2lp11.cpp 1935 2014-04-26 02:02:58Z 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 //static void xxx(double x){;}
00143 
00148 dvector safe_choleski_solver::solve
00149   (const banded_symmetric_dmatrix& _m,const dvector&_v)
00150 {
00151   int ierr=0;
00152   ADUNCONST(dvector,v)
00153   ADUNCONST(banded_symmetric_dmatrix,m)
00154   int mmin=m.indexmin();
00155   //int mmax=m.indexmax();
00156   if (hadbad && id>0.0)
00157   {
00158     positivize(m,id);
00159    dirty=1;
00160   }
00161   m.shift(1);
00162   v.shift(1);
00163   int ibreak=1;
00164   dvector w;
00165   do
00166   {
00167     const banded_lower_triangular_dmatrix& C=quiet_choleski_decomp(m,ierr);
00168     if (ierr==0)
00169     {
00170       id/=2.0;
00171       w=solve_trans(C,::solve(C,v));
00172       dvector delta=m*w;
00173       dvector err=solve_trans(C,::solve(C,v-delta));
00174       dvector w1=w+err;
00175       cout << norm(w1-w) << endl;
00176       if (norm(err)>1.e-10)
00177       {
00178         cout << "precisionerror" << endl;
00179       }
00180       angle=w*v/(norm(w)*norm(v));
00181       ibreak=0;
00182     }
00183     else
00184     {
00185       id*=2.0;
00186       positivize(m,id);
00187       ierr=0;
00188       dirty=1;
00189       hadbad=1;
00190     }
00191   }
00192   while(ibreak);
00193   m.shift(mmin);
00194   w.shift(mmin);
00195   v.shift(mmin);
00196   return w;
00197 }
00198 
00203 void laplace_approximation_calculator::
00204   do_newton_raphson_state_space(function_minimizer * pfmin,double f_from_1,
00205   int& no_converge_flag)
00206 {
00207   laplace_approximation_calculator::where_are_we_flag=2;
00208   double fbest=1.e+100;
00209   double fval=1.e+100;
00210   double maxg=fabs(evaluate_function(fbest,uhat,pfmin));
00211 
00212 
00213   laplace_approximation_calculator::where_are_we_flag=0;
00214   dvector uhat_old(1,usize);
00215   safe_choleski_solver scs(0.1);
00216   //for(int ii=1;ii<=num_nr_iters;ii++)
00217   int ii=0;
00218   do
00219   {
00220     bHess->initialize();
00221 
00222     grad.initialize();
00223 
00224     step=get_newton_raphson_info_banded(pfmin);
00225 
00226     // check for degenerate Hessian
00227     int check_hessian=0;
00228     if (check_hessian)
00229     {
00230       ofstream ofs("hh");
00231       ofs << colsum(dmatrix(*bHess)) << endl;
00232     }
00233 
00234     if (!initial_params::mc_phase)
00235       cout << "Newton raphson " << ii << "  ";
00236 
00237     if (quadratic_prior::get_num_quadratic_prior()>0)
00238     {
00239       quadratic_prior::get_cHessian_contribution(Hess,xsize);
00240       quadratic_prior::get_cgradient_contribution(grad,xsize);
00241     }
00242 
00243     //int ierr=0;
00244 
00245     dvector g1(1,usize);
00246     maxg=fabs(evaluate_function(fval,uhat,g1,pfmin));
00247     if (fval>fbest)
00248     fbest=fval;
00249     if (nr_debug==1)
00250     {
00251       cout << " grad compare " << norm(g1-grad)  << endl;
00252     }
00253     step=scs.solve(*bHess,g1);
00254     //step=scs.solve(*bHess,grad);
00255     if (nr_debug==1)
00256     {
00257       cout << " angle = " << step*grad/(norm(step)*norm(grad)) << endl;
00258     }
00259     int iic=0;
00260     double testangle=-1;
00261     int extra_try=0;
00262     dvector utry(1,usize);
00263     int smallshrink=0;
00264     do
00265     {
00266       if (++iic>10)
00267       {
00268         break;
00269       }
00270       if (extra_try==0)
00271       {
00272         utry = uhat-step;
00273       }
00274       else
00275       {
00276         utry = uhat-0.5*step;
00277       }
00278       dvector g(1,usize);
00279       maxg=fabs(evaluate_function(fval,utry,g,pfmin));
00280       if (nr_debug==1)
00281       {
00282         cout << "  fbest-fval = " << setprecision(15)
00283            <<  fbest-fval  << endl;
00284       }
00285       if (fval>fbest && maxg>1.e-10)
00286       {
00287         if (maxg<1.e-10)
00288           smallshrink=3;
00289         else if (maxg<1.e-9)
00290           smallshrink=2;
00291         else if (maxg<1.e-8)
00292           smallshrink=1;
00293 
00294         if (nr_debug==1)
00295         {
00296           testangle=g*step/(norm(g)*norm(step));
00297           cout << fval-fbest << " step too large  angle = " << testangle
00298                << endl;
00299         }
00300       }
00301       if (fval==fbest)
00302       {
00303         testangle=g*step/(norm(g)*norm(step));
00304         cout << " no progress  angle = " << testangle << endl;
00305       }
00306       if (fval<=fbest)
00307       {
00308         uhat=utry;
00309         fbest=fval;
00310         testangle=g*step/(norm(g)*norm(step));
00311         if (nr_debug==1)
00312         {
00313           cout << "inner angle = " << testangle << endl;
00314         }
00315         if (testangle>0.4)
00316         {
00317           extra_try=extra_try+1;
00318           if (nr_debug==1)
00319           {
00320             cout << extra_try << endl;
00321           }
00322         }
00323         else
00324         {
00325           break;
00326         }
00327       }
00328       else
00329       {
00330         if (extra_try>0)
00331         {
00332           break;
00333         }
00334         else
00335         {
00336           if (smallshrink==0)
00337             step/=100.0;
00338           else if(smallshrink==1)
00339             step/=10.0;
00340           else if(smallshrink==2)
00341             step/=5;
00342           else if(smallshrink==3)
00343             step/=2;
00344           smallshrink=0;
00345         }
00346       }
00347     }
00348     while(1);
00349 
00350     ii++;
00351 
00352     if (scs.dirty==1)
00353     {
00354       scs.dirty=0;
00355       step=get_newton_raphson_info_banded(pfmin);
00356       int print_hessian=0;
00357       if (print_hessian)
00358       {
00359         ofstream ofs("hh1");
00360         ofs << setw(12) << setscientific() << setprecision(3) << endl;
00361       }
00362 
00363       if (quadratic_prior::get_num_quadratic_prior()>0)
00364       {
00365         quadratic_prior::get_cHessian_contribution(Hess,xsize);
00366         quadratic_prior::get_cgradient_contribution(grad,xsize);
00367       }
00368       if (ii>=num_nr_iters || maxg < 1.e-13 )
00369       {
00370         step=scs.solve(*bHess,g1);
00371       }
00372       //solve(*bHess,grad);
00373     }
00374 
00375     for (int i=1;i<=usize;i++)
00376     {
00377       y(i+xsize)=uhat(i);
00378     }
00379 
00380     if (scs.dirty==0)
00381     {
00382       if (ii>=num_nr_iters || maxg < 1.e-13 )
00383       {
00384         break;
00385       }
00386     }
00387     else
00388     {
00389       scs.dirty=0;
00390       scs.hadbad=0;
00391       if (ii>100)
00392       {
00393         cerr << "Can't get positive definite Hessian in inner optimization"
00394              << endl;
00395         break;
00396       }
00397     }
00398   }
00399   while(1);
00400 }