ADMB Documentation  11.5.3260
 All Classes Files Functions Variables Typedefs Friends Defines
fvar_m40.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id$
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 
00248 dvariable ln_det_choleski(
00249   const banded_symmetric_dvar_matrix& MM,
00250   int& ierr)
00251 {
00252   banded_lower_triangular_dvar_matrix tmp=choleski_decomp(MM, ierr);
00253   dvariable ld=0.0;
00254   if (ierr==1)
00255   {
00256     return ld;
00257   }
00258 
00259   int mmin=tmp.indexmin();
00260   int mmax=tmp.indexmax();
00261   for (int i=mmin;i<=mmax;i++)
00262   {
00263     ld+=log(tmp(i,i));
00264   }
00265   return 2.0*ld;
00266 }
00267 banded_lower_triangular_dvar_matrix choleski_decomp(
00268   const banded_symmetric_dvar_matrix& MM,
00269   int& ierr)
00270 {
00271   // kludge to deal with constantness
00272   ierr = 0;
00273   banded_symmetric_dvar_matrix& M = (banded_symmetric_dvar_matrix&) MM;
00274   int n=M.indexmax();
00275 
00276   int bw=M.bandwidth();
00277   banded_lower_triangular_dvar_matrix L(1,n,bw);
00278 #ifndef SAFE_INITIALIZE
00279     L.initialize();
00280 #endif
00281 
00282   int i,j,k;
00283   double tmp;
00284     if (M(1,1)<=0)
00285     {
00286       cerr << "Error matrix not positive definite in choleski_decomp"
00287        " value was " << M(1,1) << " for index  1"   <<endl;
00288       ierr=1;
00289       M(1,1)=1.0;
00290     }
00291   L.elem_value(1,1)=sqrt(value(M(1,1)));
00292   for (i=2;i<=bw;i++)
00293   {
00294     L.elem_value(i,1)=value(M(i,1))/L.elem_value(1,1);
00295   }
00296 
00297   for (i=2;i<=n;i++)
00298   {
00299     int jmin=admax(2,i-bw+1);
00300     for (j=jmin;j<=i-1;j++)
00301     {
00302       tmp=value(M(i,j));
00303       int kmin=max(1,j-bw+1,i-bw+1);
00304       for (k=kmin;k<=j-1;k++)
00305       {
00306         tmp-=L.elem_value(i,k)*L.elem_value(j,k);
00307       }
00308       L.elem_value(i,j)=tmp/L.elem_value(j,j);
00309     }
00310     tmp=value(M(i,i));
00311     int kmin=admax(i-bw+1,1);
00312     //double vmax=fabs(L.elem_value(i,kmin));
00313     //for (k=kmin;k<=i-1;k++)
00314     //{
00315      // if (fabs(L.elem_value(i,k))>vmax) vmax=fabs(L.elem_value(i,k));
00316     //}
00317     //tmp/=square(vmax);
00318     for (k=kmin;k<=i-1;k++)
00319     {
00320       tmp-=L.elem_value(i,k)*L.elem_value(i,k);
00321       //tmp-=square(L.elem_value(i,k)/vmax);
00322     }
00323     if (tmp<=0)
00324     {
00325       cerr << "Error matrix not positive definite in choleski_decomp"
00326        " value was " << tmp << " for index " << i    <<endl;
00327       ierr=1;
00328 
00329 #ifdef DIAG
00330       int print_switch=0;
00331       if (print_switch)
00332       {
00333         dmatrix CMM=dmatrix(value(MM));
00334         ofstream ofs("hh");
00335         {
00336           ofs << setprecision(3) << setscientific() << setw(11)
00337               << CMM << endl<< endl;
00338         }
00339         dvector ev(CMM.indexmin(),CMM.indexmax());
00340         dmatrix evec=eigenvectors(CMM,ev);
00341         ofs << setprecision(3) << setscientific() << setw(11)
00342               << ev << endl<< endl;
00343         ofs << setprecision(3) << setscientific() << setw(11)
00344               << evec << endl<< endl;
00345         uostream uos("uos");
00346         uos << CMM.indexmax()-CMM.indexmin()+1;
00347         uos << CMM;
00348       }
00349 #endif
00350       /*
00351       dmatrix N(1,4,1,4);
00352 
00353       for (int i=1;i<=4;i++)
00354       {
00355         N(i,i)=value(M(i,i));
00356         for (int j=1;j<i;j++)
00357         {
00358           N(i,j)=value(M(i,j));
00359           N(j,i)=value(M(i,j));
00360         }
00361       }
00362       cout << N << endl;
00363       cout << eigenvalues(N) << endl;
00364       cout << choleski_decomp(N) << endl;
00365       */
00366 
00367       tmp=1.0;
00368     }
00369     //L.elem_value(i,i)=vmax*sqrt(tmp);
00370     L.elem_value(i,i)=sqrt(tmp);
00371   }
00372 
00373   //banded_lower_triangular_dvar_matrix vc=nograd_assign(L);
00374   save_identifier_string("rs");
00375   L.save_dvar_matrix_position();
00376   save_identifier_string("rt");
00377   MM.save_dvar_matrix_value();
00378   save_identifier_string("rl");
00379   MM.save_dvar_matrix_position();
00380   save_identifier_string("ro");
00381   gradient_structure::GRAD_STACK1->
00382       set_gradient_stack(dfcholeski_decomp_banded);
00383 
00384   return L;
00385 }
00386 
00391 void dfcholeski_decomp_banded(void)
00392 {
00393   verify_identifier_string("ro");
00394   dvar_matrix_position MMpos=restore_dvar_matrix_position();
00395   verify_identifier_string("rl");
00396   banded_symmetric_dmatrix M=
00397     restore_banded_symmetric_dvar_matrix_value(MMpos);
00398   verify_identifier_string("rt");
00399   dvar_matrix_position vcpos=restore_dvar_matrix_position();
00400   verify_identifier_string("rs");
00401   banded_lower_triangular_dmatrix dfL=
00402     restore_banded_lower_triangular_dvar_matrix_derivatives(vcpos);
00403 
00404   int n=M.indexmax();
00405   int bw=M.bandwidth();
00406 
00407   banded_lower_triangular_dmatrix L(1,n,bw);
00408   banded_lower_triangular_dmatrix tmp1(1,n,bw);
00409   banded_lower_triangular_dmatrix dftmp1(1,n,bw);
00410   banded_symmetric_dmatrix dfM(1,n,bw);
00411   dvector tmp(1,n);
00412   dvector dftmp(1,n);
00413   tmp.initialize();
00414   tmp1.initialize();
00415   dftmp.initialize();
00416   dftmp1.initialize();
00417   dfM.initialize();
00418 #ifndef SAFE_INITIALIZE
00419     L.initialize();
00420 #endif
00421 
00422   int i,j,k;
00423   if (M(1,1)<=0)
00424   {
00425     cerr << "Error matrix not positive definite in choleski_decomp"
00426       <<endl;
00427     M(1,1)=1.0;
00428     //ad_exit(1);
00429   }
00430   L(1,1)=sqrt(M(1,1));
00431   for (i=2;i<=bw;i++)
00432   {
00433     L(i,1)=M(i,1)/L(1,1);
00434   }
00435 
00436   for (i=2;i<=n;i++)
00437   {
00438     int jmin=admax(2,i-bw+1);
00439     for (j=jmin;j<=i-1;j++)
00440     {
00441       tmp1(i,j)=M(i,j);
00442       int kmin=max(1,j-bw+1,i-bw+1);
00443       for (k=kmin;k<=j-1;k++)
00444       {
00445         tmp1(i,j)-=L(i,k)*L(j,k);
00446       }
00447       L(i,j)=tmp1(i,j)/L(j,j);
00448     }
00449     tmp(i)=M(i,i);
00450     int kmin=admax(i-bw+1,1);
00451     //double vmax=fabs(L(i,kmin));
00452     //for (k=kmin;k<=i-1;k++)
00453    // {
00454      // if (fabs(L(i,k))>vmax) vmax=fabs(L(i,k));
00455    // }
00456     //tmp(i)/=square(vmax);
00457     for (k=kmin;k<=i-1;k++)
00458     {
00459       tmp(i)-=L(i,k)*L(i,k);
00460       //tmp(i)-=square(L(i,k)/vmax);
00461     }
00462     if (tmp(i)<=0)
00463     {
00464       cerr << "Error matrix not positive definite in choleski_decomp"
00465         <<endl;
00466       tmp(i)=1.0;
00467       //ad_exit(1);
00468     }
00469     //L(i,i)=vmax*sqrt(tmp(i));
00470     L(i,i)=sqrt(tmp(i));
00471   }
00472  //*******************************************************************8
00473  //*******************************************************************8
00474  //*******************************************************************8
00475   for (i=n;i>=2;i--)
00476   {
00477     //L(i,i)=sqrt(tmp(i));
00478     dftmp(i)+=dfL(i,i)/(2.0*L(i,i));
00479     dfL(i,i)=0.0;
00480     int kmin=admax(i-bw+1,1);
00481     for (k=i-1;k>=kmin;k--)
00482     {
00483       //tmp(i)-=L(i,k)*L(i,k);
00484       dfL(i,k)-=2.*dftmp(i)*L(i,k);
00485     }
00486     //tmp(i)=M(i,i);
00487     dfM(i,i)+=dftmp(i);
00488     dftmp(i)=0.0;
00489     int jmin=admax(2,i-bw+1);
00490     for (j=i-1;j>=jmin;j--)
00491     {
00492       //L(i,j)=tmp1(i,j)/L(j,j);
00493       double linv=1./L(j,j);
00494       dftmp1(i,j)+=dfL(i,j)*linv;
00495       dfL(j,j)-=dfL(i,j)*tmp1(i,j)*linv*linv;
00496       dfL(i,j)=0.0;
00497       kmin=max(1,j-bw+1,i-bw+1);
00498       for (k=j-1;k>=kmin;k--)
00499       {
00500         //tmp(i,j)-=L(i,k)*L(j,k);
00501         dfL(i,k)-=dftmp1(i,j)*L(j,k);
00502         dfL(j,k)-=dftmp1(i,j)*L(i,k);
00503       }
00504       //tmp(i,j)=M(i,j);
00505       dfM(i,j)+=dftmp1(i,j);
00506       dftmp1(i,j)=0.0;
00507     }
00508   }
00509   double linv=1./L(1,1);
00510   for (i=bw;i>=2;i--)
00511   {
00512     //L(i,1)=M(i,1)/L(1,1);
00513     dfM(i,1)+=dfL(i,1)*linv;
00514     dfL(1,1)-=dfL(i,1)*M(i,1)*linv*linv;
00515     dfL(i,1)=0.0;
00516   }
00517   //L(1,1)=sqrt(M(1,1));
00518   dfM(1,1)+=dfL(1,1)/(2.*L(1,1));
00519 
00520 
00521  //*******************************************************************8
00522  //*******************************************************************8
00523  //*******************************************************************8
00524 
00525   dfM.save_dmatrix_derivatives(MMpos);
00526 }
00527 
00532 dvar_matrix::dvar_matrix(const banded_symmetric_dvar_matrix& S1)
00533 {
00534   banded_symmetric_dvar_matrix& S= (banded_symmetric_dvar_matrix&) S1;
00535   int imin=S.indexmin();
00536   int imax=S.indexmax();
00537   int bw=S.bandwidth();
00538   allocate(imin,imax,imin,imax);
00539   int i1;
00540   int j1;
00541   initialize();
00542   for (int i=imin;i<=imax;i++)
00543   {
00544     for (int j=imin;j<=imax;j++)
00545     {
00546       if (j<=i)
00547       {
00548         j1=j;
00549         i1=i;
00550       }
00551       else
00552       {
00553         j1=i;
00554         i1=j;
00555       }
00556       if ( (i1-j1) < bw)
00557         (*this)(i,j)=S(i1,j1);
00558     }
00559   }
00560 }
00561 
00566 dvar_matrix::dvar_matrix(const banded_lower_triangular_dvar_matrix& S1)
00567 {
00568   banded_lower_triangular_dvar_matrix& S=
00569     (banded_lower_triangular_dvar_matrix&) S1;
00570   int imin=S.indexmin();
00571   int imax=S.indexmax();
00572   int bw=S.bandwidth();
00573   allocate(imin,imax,imin,imax);
00574   initialize();
00575   for (int i=imin;i<=imax;i++)
00576   {
00577     for (int j=imin;j<=imax;j++)
00578     {
00579       if (j<=i)
00580       {
00581         if ( (i-j) < bw)
00582           (*this)(i,j)=S(i,j);
00583       }
00584     }
00585   }
00586 }
00587 
00592 int max(int i,int j,int k)
00593 {
00594   if (i>j)
00595     if ( i>k)
00596       return i;
00597     else
00598       return k;
00599   else
00600     if ( j>k)
00601       return j;
00602     else
00603       return k;
00604 }
00605 
00610 dmatrix restore_lower_triangular_dvar_matrix_value(
00611   const dvar_matrix_position& mpos)
00612 {
00613   // restores the size, address, and value information for a dvar_matrix
00614   banded_lower_triangular_dmatrix out((const dvar_matrix_position&)mpos);
00615   //int ierr;
00616   int min=out.rowmin();
00617   int max=out.rowmax();
00618   for (int i=max;i>=min;i--)
00619   {
00620     dvar_vector_position vpos=restore_dvar_vector_position();
00621     out(i)=restore_dvar_vector_value(vpos);
00622   }
00623   return out;
00624 }
00625 
00630 void check_choleski_decomp(const banded_symmetric_dvar_matrix& MM,
00631   const int& _ierr)
00632 {
00633   int& ierr=(int&)(_ierr);
00634   // kludge to deal with constantness
00635   ierr =0;
00636   banded_symmetric_dvar_matrix& M = (banded_symmetric_dvar_matrix&) MM;
00637   int n=M.indexmax();
00638 
00639   int bw=M.bandwidth();
00640   banded_lower_triangular_dvar_matrix L(1,n,bw);
00641 #ifndef SAFE_INITIALIZE
00642     L.initialize();
00643 #endif
00644 
00645   int i,j,k;
00646   double tmp;
00647     if (M(1,1)<=0)
00648     {
00649       cerr << "Error matrix not positive definite in choleski_decomp"
00650        " value was " << M(1,1) << " for index  1"   <<endl;
00651       ierr=1;
00652       return;
00653       //M(1,1)=1.0;
00654     }
00655   L.elem_value(1,1)=sqrt(value(M(1,1)));
00656   for (i=2;i<=bw;i++)
00657   {
00658     L.elem_value(i,1)=value(M(i,1))/L.elem_value(1,1);
00659   }
00660 
00661   for (i=2;i<=n;i++)
00662   {
00663     int jmin=admax(2,i-bw+1);
00664     for (j=jmin;j<=i-1;j++)
00665     {
00666       tmp=value(M(i,j));
00667       int kmin=max(1,j-bw+1,i-bw+1);
00668       for (k=kmin;k<=j-1;k++)
00669       {
00670         tmp-=L.elem_value(i,k)*L.elem_value(j,k);
00671       }
00672       L.elem_value(i,j)=tmp/L.elem_value(j,j);
00673     }
00674     tmp=value(M(i,i));
00675     int kmin=admax(i-bw+1,1);
00676     for (k=kmin;k<=i-1;k++)
00677     {
00678       tmp-=L.elem_value(i,k)*L.elem_value(i,k);
00679     }
00680     if (tmp<=0)
00681     {
00682       cerr << "Error matrix not positive definite in choleski_decomp"
00683        " value was " << tmp << " for index " << i    <<endl;
00684       ierr=1;
00685       return;
00686       //tmp=1.0;
00687     }
00688     L.elem_value(i,i)=sqrt(tmp);
00689   }
00690 }
00691 
00696 dvariable norm(const banded_symmetric_dvar_matrix& B)
00697 {
00698   return sqrt(norm2(B));
00699 }
00700 
00705 dvariable norm2(const banded_symmetric_dvar_matrix& B)
00706 {
00707   dvariable nm=0.0;
00708   for (int i=1;i<=B.bw-1;i++)
00709   {
00710     nm+=norm2(B.d(i));
00711   }
00712   nm*=2;
00713   nm+=norm2(B.d(0));
00714   return nm;
00715 }
00716 dvariable sumsq(const banded_symmetric_dvar_matrix& B) {return(norm2(B));}