ADMB Documentation  11.2.2822
 All Classes Files Functions Variables Typedefs Friends Defines
fvar_m40.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id: fvar_m40.cpp 2728 2014-11-25 22:52:42Z johnoel $
00003  *
00004  * Author: David Fournier
00005  * Copyright (c) 2008-2012 Regents of the University of California
00006  */
00011 #include "fvar.hpp"
00012 
00017 banded_lower_triangular_dvar_matrix::banded_lower_triangular_dvar_matrix
00018    (const banded_lower_triangular_dvar_matrix& S) : bw(S.bw), d(S.d)
00019 {}
00020 
00025 banded_symmetric_dvar_matrix::banded_symmetric_dvar_matrix
00026    (const banded_symmetric_dvar_matrix& S) : bw(S.bw), d(S.d)
00027 {}
00028 
00029 #if !defined(OPT_LIB)
00030 
00034 dvar_vector banded_symmetric_dvar_matrix::operator () (int i)
00035 {
00036   return d(i);
00037 }
00038 
00043 prevariable banded_symmetric_dvar_matrix::operator () (int i,int j)
00044 {
00045   return d(i-j,i);
00046 }
00047 
00052 prevariable banded_lower_triangular_dvar_matrix::operator () (int i,int j)
00053 {
00054   return d(i-j,i);
00055 }
00056 
00057 const prevariable banded_lower_triangular_dvar_matrix::operator()(int i, int j)
00058   const
00059 {
00060   return d(i-j,i);
00061 }
00062 
00067 dvar_vector banded_lower_triangular_dvar_matrix::operator () (int i)
00068 {
00069   return d(i);
00070 }
00071 #endif
00072 
00077 void banded_symmetric_dvar_matrix::initialize(void)
00078 {
00079   for (int i=rowmin();i<=rowmax();i++)
00080   {
00081     (*this)(i).initialize();
00082   }
00083 }
00084 
00089 void banded_lower_triangular_dvar_matrix::initialize(void)
00090 {
00091   for (int i=rowmin();i<=rowmax();i++)
00092   {
00093     (*this)(i).initialize();
00094   }
00095 }
00096 
00101 banded_symmetric_dvar_matrix::banded_symmetric_dvar_matrix
00102   (int _min,int _max,int _bw)
00103 {
00104   bw=_bw;
00105   ivector  lb(0,_bw-1);
00106   lb.fill_seqadd(_min,1);
00107   d.allocate(0,_bw-1,lb,_max);
00108 }
00109 
00114 banded_symmetric_dmatrix value(const banded_symmetric_dvar_matrix &v)
00115 {
00116   int bw=v.bandwidth();
00117   banded_symmetric_dmatrix w(v.indexmin(),v.indexmax(),v.bw);
00118   for (int i=0;i<=bw-1;i++)
00119   {
00120     w.d(i)=value(v.d(i));
00121   }
00122   return w;
00123 }
00124 
00129 banded_lower_triangular_dmatrix value
00130   (const banded_lower_triangular_dvar_matrix &v)
00131 {
00132   int bw=v.bandwidth();
00133   banded_lower_triangular_dmatrix w(v.indexmin(),v.indexmax(),v.bw);
00134   for (int i=0;i<=bw-1;i++)
00135   {
00136     w.d(i)=value(v.d(i));
00137   }
00138   return w;
00139 }
00140 
00145 ostream& operator<<(const ostream& _ofs, const banded_symmetric_dvar_matrix& S1)
00146 {
00147   ostream& ofs= (ostream&) _ofs;
00148   banded_symmetric_dvar_matrix& S= (banded_symmetric_dvar_matrix&) S1;
00149 
00150   int imin=S.indexmin();
00151   int imax=S.indexmax();
00152   int bw=S.bandwidth();
00153   int i1;
00154   int j1;
00155   for (int i=imin;i<=imax;i++)
00156   {
00157     for (int j=imin;j<=imax;j++)
00158     {
00159       if (j<i)
00160       {
00161         j1=j;
00162         i1=i;
00163       }
00164       else
00165       {
00166         j1=i;
00167         i1=j;
00168       }
00169       if ( (i1-j1) < bw)
00170         ofs << S(i1,j1) << " ";
00171       else
00172         ofs << 0.0 << " ";
00173     }
00174     if (i<imax) ofs << endl;
00175   }
00176   return (ostream&)ofs;
00177 }
00178 
00183 banded_lower_triangular_dvar_matrix::banded_lower_triangular_dvar_matrix
00184   (int _min,int _max,int _bw)
00185 {
00186   bw=_bw;
00187   ivector  lb(0,_bw-1);
00188   lb.fill_seqadd(_min,1);
00189   d.allocate(0,_bw-1,lb,_max);
00190 }
00191 
00196 ostream& operator<<(const ostream& _ofs,
00197   const banded_lower_triangular_dvar_matrix& S1)
00198 {
00199   ostream& ofs= (ostream&) _ofs;
00200   banded_lower_triangular_dvar_matrix& S =
00201     (banded_lower_triangular_dvar_matrix&)S1;
00202   int imin=S.indexmin();
00203   int imax=S.indexmax();
00204   int bw=S.bandwidth();
00205   for (int i=imin;i<=imax;i++)
00206   {
00207     for (int j=imin;j<=imax;j++)
00208     {
00209       if (j<=i)
00210       {
00211         if ( (i-j) < bw)
00212           ofs << S(i,j) << " ";
00213         else
00214           ofs << 0.0 << " ";
00215       }
00216       else
00217       {
00218         ofs << 0.0 << " ";
00219       }
00220     }
00221     if (i<imax) ofs << endl;
00222   }
00223   return (ostream&)ofs;
00224 }
00225 
00226 
00227 
00228 #include "fvar.hpp"
00229 
00230 #ifdef __TURBOC__
00231 #pragma hdrstop
00232 #include <iostream.h>
00233 #endif
00234 
00235 #ifdef __ZTC__
00236 #include <iostream.hpp>
00237 #endif
00238 
00239 #ifdef __SUN__
00240 #include <iostream.h>
00241 #endif
00242 #ifdef __NDPX__
00243 #include <iostream.h>
00244 #endif
00245 void dfcholeski_decomp(void);
00246 void dfcholeski_decomp_banded(void);
00247 
00252 dvariable ln_det_choleski(
00253   const banded_symmetric_dvar_matrix& MM, const int& _ierr)
00254 {
00255   banded_lower_triangular_dvar_matrix tmp=choleski_decomp(MM,_ierr);
00256   dvariable ld=0.0;
00257   if (_ierr==1)
00258   {
00259     return ld;
00260   }
00261 
00262   int mmin=tmp.indexmin();
00263   int mmax=tmp.indexmax();
00264   for (int i=mmin;i<=mmax;i++)
00265   {
00266     ld+=log(tmp(i,i));
00267   }
00268   return 2.0*ld;
00269 }
00270 
00275 banded_lower_triangular_dvar_matrix choleski_decomp(
00276   const banded_symmetric_dvar_matrix& MM, const int& _ierr)
00277 {
00278   int& ierr=(int&)(_ierr);
00279   // kludge to deal with constantness
00280   ierr =0;
00281   banded_symmetric_dvar_matrix& M = (banded_symmetric_dvar_matrix&) MM;
00282   int n=M.indexmax();
00283 
00284   int bw=M.bandwidth();
00285   banded_lower_triangular_dvar_matrix L(1,n,bw);
00286 #ifndef SAFE_INITIALIZE
00287     L.initialize();
00288 #endif
00289 
00290   int i,j,k;
00291   double tmp;
00292     if (M(1,1)<=0)
00293     {
00294       cerr << "Error matrix not positive definite in choleski_decomp"
00295        " value was " << M(1,1) << " for index  1"   <<endl;
00296       ierr=1;
00297       M(1,1)=1.0;
00298     }
00299   L.elem_value(1,1)=sqrt(value(M(1,1)));
00300   for (i=2;i<=bw;i++)
00301   {
00302     L.elem_value(i,1)=value(M(i,1))/L.elem_value(1,1);
00303   }
00304 
00305   for (i=2;i<=n;i++)
00306   {
00307     int jmin=admax(2,i-bw+1);
00308     for (j=jmin;j<=i-1;j++)
00309     {
00310       tmp=value(M(i,j));
00311       int kmin=max(1,j-bw+1,i-bw+1);
00312       for (k=kmin;k<=j-1;k++)
00313       {
00314         tmp-=L.elem_value(i,k)*L.elem_value(j,k);
00315       }
00316       L.elem_value(i,j)=tmp/L.elem_value(j,j);
00317     }
00318     tmp=value(M(i,i));
00319     int kmin=admax(i-bw+1,1);
00320     //double vmax=fabs(L.elem_value(i,kmin));
00321     //for (k=kmin;k<=i-1;k++)
00322     //{
00323      // if (fabs(L.elem_value(i,k))>vmax) vmax=fabs(L.elem_value(i,k));
00324     //}
00325     //tmp/=square(vmax);
00326     for (k=kmin;k<=i-1;k++)
00327     {
00328       tmp-=L.elem_value(i,k)*L.elem_value(i,k);
00329       //tmp-=square(L.elem_value(i,k)/vmax);
00330     }
00331     if (tmp<=0)
00332     {
00333       cerr << "Error matrix not positive definite in choleski_decomp"
00334        " value was " << tmp << " for index " << i    <<endl;
00335       ierr=1;
00336 
00337       int print_switch=0;
00338       if (print_switch)
00339       {
00340         dmatrix CMM=dmatrix(value(MM));
00341         ofstream ofs("hh");
00342         {
00343           ofs << setprecision(3) << setscientific() << setw(11)
00344               << CMM << endl<< endl;
00345         }
00346         dvector ev(CMM.indexmin(),CMM.indexmax());
00347         dmatrix evec=eigenvectors(CMM,ev);
00348         ofs << setprecision(3) << setscientific() << setw(11)
00349               << ev << endl<< endl;
00350         ofs << setprecision(3) << setscientific() << setw(11)
00351               << evec << endl<< endl;
00352         uostream uos("uos");
00353         uos << CMM.indexmax()-CMM.indexmin()+1;
00354         uos << CMM;
00355       }
00356       /*
00357       dmatrix N(1,4,1,4);
00358 
00359       for (int i=1;i<=4;i++)
00360       {
00361         N(i,i)=value(M(i,i));
00362         for (int j=1;j<i;j++)
00363         {
00364           N(i,j)=value(M(i,j));
00365           N(j,i)=value(M(i,j));
00366         }
00367       }
00368       cout << N << endl;
00369       cout << eigenvalues(N) << endl;
00370       cout << choleski_decomp(N) << endl;
00371       */
00372 
00373       tmp=1.0;
00374     }
00375     //L.elem_value(i,i)=vmax*sqrt(tmp);
00376     L.elem_value(i,i)=sqrt(tmp);
00377   }
00378 
00379   //banded_lower_triangular_dvar_matrix vc=nograd_assign(L);
00380   save_identifier_string("rs");
00381   L.save_dvar_matrix_position();
00382   save_identifier_string("rt");
00383   MM.save_dvar_matrix_value();
00384   save_identifier_string("rl");
00385   MM.save_dvar_matrix_position();
00386   save_identifier_string("ro");
00387   gradient_structure::GRAD_STACK1->
00388       set_gradient_stack(dfcholeski_decomp_banded);
00389 
00390   return L;
00391 }
00392 
00397 void dfcholeski_decomp_banded(void)
00398 {
00399   verify_identifier_string("ro");
00400   dvar_matrix_position MMpos=restore_dvar_matrix_position();
00401   verify_identifier_string("rl");
00402   banded_symmetric_dmatrix M=
00403     restore_banded_symmetric_dvar_matrix_value(MMpos);
00404   verify_identifier_string("rt");
00405   dvar_matrix_position vcpos=restore_dvar_matrix_position();
00406   verify_identifier_string("rs");
00407   banded_lower_triangular_dmatrix dfL=
00408     restore_banded_lower_triangular_dvar_matrix_derivatives(vcpos);
00409 
00410   int n=M.indexmax();
00411   int bw=M.bandwidth();
00412 
00413   banded_lower_triangular_dmatrix L(1,n,bw);
00414   banded_lower_triangular_dmatrix tmp1(1,n,bw);
00415   banded_lower_triangular_dmatrix dftmp1(1,n,bw);
00416   banded_symmetric_dmatrix dfM(1,n,bw);
00417   dvector tmp(1,n);
00418   dvector dftmp(1,n);
00419   tmp.initialize();
00420   tmp1.initialize();
00421   dftmp.initialize();
00422   dftmp1.initialize();
00423   dfM.initialize();
00424 #ifndef SAFE_INITIALIZE
00425     L.initialize();
00426 #endif
00427 
00428   int i,j,k;
00429   if (M(1,1)<=0)
00430   {
00431     cerr << "Error matrix not positive definite in choleski_decomp"
00432       <<endl;
00433     M(1,1)=1.0;
00434     //ad_exit(1);
00435   }
00436   L(1,1)=sqrt(M(1,1));
00437   for (i=2;i<=bw;i++)
00438   {
00439     L(i,1)=M(i,1)/L(1,1);
00440   }
00441 
00442   for (i=2;i<=n;i++)
00443   {
00444     int jmin=admax(2,i-bw+1);
00445     for (j=jmin;j<=i-1;j++)
00446     {
00447       tmp1(i,j)=M(i,j);
00448       int kmin=max(1,j-bw+1,i-bw+1);
00449       for (k=kmin;k<=j-1;k++)
00450       {
00451         tmp1(i,j)-=L(i,k)*L(j,k);
00452       }
00453       L(i,j)=tmp1(i,j)/L(j,j);
00454     }
00455     tmp(i)=M(i,i);
00456     int kmin=admax(i-bw+1,1);
00457     //double vmax=fabs(L(i,kmin));
00458     //for (k=kmin;k<=i-1;k++)
00459    // {
00460      // if (fabs(L(i,k))>vmax) vmax=fabs(L(i,k));
00461    // }
00462     //tmp(i)/=square(vmax);
00463     for (k=kmin;k<=i-1;k++)
00464     {
00465       tmp(i)-=L(i,k)*L(i,k);
00466       //tmp(i)-=square(L(i,k)/vmax);
00467     }
00468     if (tmp(i)<=0)
00469     {
00470       cerr << "Error matrix not positive definite in choleski_decomp"
00471         <<endl;
00472       tmp(i)=1.0;
00473       //ad_exit(1);
00474     }
00475     //L(i,i)=vmax*sqrt(tmp(i));
00476     L(i,i)=sqrt(tmp(i));
00477   }
00478  //*******************************************************************8
00479  //*******************************************************************8
00480  //*******************************************************************8
00481   for (i=n;i>=2;i--)
00482   {
00483     //L(i,i)=sqrt(tmp(i));
00484     dftmp(i)+=dfL(i,i)/(2.0*L(i,i));
00485     dfL(i,i)=0.0;
00486     int kmin=admax(i-bw+1,1);
00487     for (k=i-1;k>=kmin;k--)
00488     {
00489       //tmp(i)-=L(i,k)*L(i,k);
00490       dfL(i,k)-=2.*dftmp(i)*L(i,k);
00491     }
00492     //tmp(i)=M(i,i);
00493     dfM(i,i)+=dftmp(i);
00494     dftmp(i)=0.0;
00495     int jmin=admax(2,i-bw+1);
00496     for (j=i-1;j>=jmin;j--)
00497     {
00498       //L(i,j)=tmp1(i,j)/L(j,j);
00499       double linv=1./L(j,j);
00500       dftmp1(i,j)+=dfL(i,j)*linv;
00501       dfL(j,j)-=dfL(i,j)*tmp1(i,j)*linv*linv;
00502       dfL(i,j)=0.0;
00503       kmin=max(1,j-bw+1,i-bw+1);
00504       for (k=j-1;k>=kmin;k--)
00505       {
00506         //tmp(i,j)-=L(i,k)*L(j,k);
00507         dfL(i,k)-=dftmp1(i,j)*L(j,k);
00508         dfL(j,k)-=dftmp1(i,j)*L(i,k);
00509       }
00510       //tmp(i,j)=M(i,j);
00511       dfM(i,j)+=dftmp1(i,j);
00512       dftmp1(i,j)=0.0;
00513     }
00514   }
00515   double linv=1./L(1,1);
00516   for (i=bw;i>=2;i--)
00517   {
00518     //L(i,1)=M(i,1)/L(1,1);
00519     dfM(i,1)+=dfL(i,1)*linv;
00520     dfL(1,1)-=dfL(i,1)*M(i,1)*linv*linv;
00521     dfL(i,1)=0.0;
00522   }
00523   //L(1,1)=sqrt(M(1,1));
00524   dfM(1,1)+=dfL(1,1)/(2.*L(1,1));
00525 
00526 
00527  //*******************************************************************8
00528  //*******************************************************************8
00529  //*******************************************************************8
00530 
00531   dfM.save_dmatrix_derivatives(MMpos);
00532 }
00533 
00538 dvar_matrix::dvar_matrix(const banded_symmetric_dvar_matrix& S1)
00539 {
00540   banded_symmetric_dvar_matrix& S= (banded_symmetric_dvar_matrix&) S1;
00541   int imin=S.indexmin();
00542   int imax=S.indexmax();
00543   int bw=S.bandwidth();
00544   allocate(imin,imax,imin,imax);
00545   int i1;
00546   int j1;
00547   initialize();
00548   for (int i=imin;i<=imax;i++)
00549   {
00550     for (int j=imin;j<=imax;j++)
00551     {
00552       if (j<=i)
00553       {
00554         j1=j;
00555         i1=i;
00556       }
00557       else
00558       {
00559         j1=i;
00560         i1=j;
00561       }
00562       if ( (i1-j1) < bw)
00563         (*this)(i,j)=S(i1,j1);
00564     }
00565   }
00566 }
00567 
00572 dvar_matrix::dvar_matrix(const banded_lower_triangular_dvar_matrix& S1)
00573 {
00574   banded_lower_triangular_dvar_matrix& S=
00575     (banded_lower_triangular_dvar_matrix&) S1;
00576   int imin=S.indexmin();
00577   int imax=S.indexmax();
00578   int bw=S.bandwidth();
00579   allocate(imin,imax,imin,imax);
00580   initialize();
00581   for (int i=imin;i<=imax;i++)
00582   {
00583     for (int j=imin;j<=imax;j++)
00584     {
00585       if (j<=i)
00586       {
00587         if ( (i-j) < bw)
00588           (*this)(i,j)=S(i,j);
00589       }
00590     }
00591   }
00592 }
00593 
00598 int max(int i,int j,int k)
00599 {
00600   if (i>j)
00601     if ( i>k)
00602       return i;
00603     else
00604       return k;
00605   else
00606     if ( j>k)
00607       return j;
00608     else
00609       return k;
00610 }
00611 
00616 dmatrix restore_lower_triangular_dvar_matrix_value(
00617   const dvar_matrix_position& mpos)
00618 {
00619   // restores the size, address, and value information for a dvar_matrix
00620   banded_lower_triangular_dmatrix out((const dvar_matrix_position&)mpos);
00621   //int ierr;
00622   int min=out.rowmin();
00623   int max=out.rowmax();
00624   for (int i=max;i>=min;i--)
00625   {
00626     dvar_vector_position vpos=restore_dvar_vector_position();
00627     out(i)=restore_dvar_vector_value(vpos);
00628   }
00629   return out;
00630 }
00631 
00636 void check_choleski_decomp(const banded_symmetric_dvar_matrix& MM,
00637   const int& _ierr)
00638 {
00639   int& ierr=(int&)(_ierr);
00640   // kludge to deal with constantness
00641   ierr =0;
00642   banded_symmetric_dvar_matrix& M = (banded_symmetric_dvar_matrix&) MM;
00643   int n=M.indexmax();
00644 
00645   int bw=M.bandwidth();
00646   banded_lower_triangular_dvar_matrix L(1,n,bw);
00647 #ifndef SAFE_INITIALIZE
00648     L.initialize();
00649 #endif
00650 
00651   int i,j,k;
00652   double tmp;
00653     if (M(1,1)<=0)
00654     {
00655       cerr << "Error matrix not positive definite in choleski_decomp"
00656        " value was " << M(1,1) << " for index  1"   <<endl;
00657       ierr=1;
00658       return;
00659       //M(1,1)=1.0;
00660     }
00661   L.elem_value(1,1)=sqrt(value(M(1,1)));
00662   for (i=2;i<=bw;i++)
00663   {
00664     L.elem_value(i,1)=value(M(i,1))/L.elem_value(1,1);
00665   }
00666 
00667   for (i=2;i<=n;i++)
00668   {
00669     int jmin=admax(2,i-bw+1);
00670     for (j=jmin;j<=i-1;j++)
00671     {
00672       tmp=value(M(i,j));
00673       int kmin=max(1,j-bw+1,i-bw+1);
00674       for (k=kmin;k<=j-1;k++)
00675       {
00676         tmp-=L.elem_value(i,k)*L.elem_value(j,k);
00677       }
00678       L.elem_value(i,j)=tmp/L.elem_value(j,j);
00679     }
00680     tmp=value(M(i,i));
00681     int kmin=admax(i-bw+1,1);
00682     for (k=kmin;k<=i-1;k++)
00683     {
00684       tmp-=L.elem_value(i,k)*L.elem_value(i,k);
00685     }
00686     if (tmp<=0)
00687     {
00688       cerr << "Error matrix not positive definite in choleski_decomp"
00689        " value was " << tmp << " for index " << i    <<endl;
00690       ierr=1;
00691       return;
00692       //tmp=1.0;
00693     }
00694     L.elem_value(i,i)=sqrt(tmp);
00695   }
00696 }
00697 
00702 dvariable norm(const banded_symmetric_dvar_matrix& B)
00703 {
00704   return sqrt(norm2(B));
00705 }
00706 
00711 dvariable norm2(const banded_symmetric_dvar_matrix& B)
00712 {
00713   dvariable nm=0.0;
00714   for (int i=1;i<=B.bw-1;i++)
00715   {
00716     nm+=norm2(B.d(i));
00717   }
00718   nm*=2;
00719   nm+=norm2(B.d(0));
00720   return nm;
00721 }
00722 dvariable sumsq(const banded_symmetric_dvar_matrix& B) {return(norm2(B));}