ADMB Documentation  11.1.1890
 All Classes Files Functions Variables Typedefs Friends Defines
fvar_m40.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id: fvar_m40.cpp 1717 2014-03-03 21:00:03Z 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 
00073 /*
00074 void banded_symmetric_dmatrix::initialize(void)
00075 {
00076   for (int i=rowmin();i<=rowmax();i++)
00077   {
00078     (*this)(i).initialize();
00079   }
00080 }
00081 
00082 void banded_lower_triangular_dmatrix::initialize(void)
00083 {
00084   for (int i=rowmin();i<=rowmax();i++)
00085   {
00086     (*this)(i).initialize();
00087   }
00088 }
00089 */
00090 
00095 void banded_symmetric_dvar_matrix::initialize(void)
00096 {
00097   for (int i=rowmin();i<=rowmax();i++)
00098   {
00099     (*this)(i).initialize();
00100   }
00101 }
00102 
00107 void banded_lower_triangular_dvar_matrix::initialize(void)
00108 {
00109   for (int i=rowmin();i<=rowmax();i++)
00110   {
00111     (*this)(i).initialize();
00112   }
00113 }
00114 
00119 banded_symmetric_dvar_matrix::banded_symmetric_dvar_matrix
00120   (int _min,int _max,int _bw)
00121 {
00122   bw=_bw;
00123   ivector  lb(0,_bw-1);
00124   lb.fill_seqadd(_min,1);
00125   d.allocate(0,_bw-1,lb,_max);
00126 }
00127 
00132 banded_symmetric_dmatrix value(const banded_symmetric_dvar_matrix &v)
00133 {
00134   int bw=v.bandwidth();
00135   banded_symmetric_dmatrix w(v.indexmin(),v.indexmax(),v.bw);
00136   for (int i=0;i<=bw-1;i++)
00137   {
00138     w.d(i)=value(v.d(i));
00139   }
00140   return w;
00141 }
00142 
00147 banded_lower_triangular_dmatrix value
00148   (const banded_lower_triangular_dvar_matrix &v)
00149 {
00150   int bw=v.bandwidth();
00151   banded_lower_triangular_dmatrix w(v.indexmin(),v.indexmax(),v.bw);
00152   for (int i=0;i<=bw-1;i++)
00153   {
00154     w.d(i)=value(v.d(i));
00155   }
00156   return w;
00157 }
00158 
00163 ostream& operator<<(const ostream& _ofs, const banded_symmetric_dvar_matrix& S1)
00164 {
00165   ostream& ofs= (ostream&) _ofs;
00166   banded_symmetric_dvar_matrix& S= (banded_symmetric_dvar_matrix&) S1;
00167 
00168   int imin=S.indexmin();
00169   int imax=S.indexmax();
00170   int bw=S.bandwidth();
00171   int i1;
00172   int j1;
00173   for (int i=imin;i<=imax;i++)
00174   {
00175     for (int j=imin;j<=imax;j++)
00176     {
00177       if (j<i)
00178       {
00179         j1=j;
00180         i1=i;
00181       }
00182       else
00183       {
00184         j1=i;
00185         i1=j;
00186       }
00187       if ( (i1-j1) < bw)
00188         ofs << S(i1,j1) << " ";
00189       else
00190         ofs << 0.0 << " ";
00191     }
00192     if (i<imax) ofs << endl;
00193   }
00194   return (ostream&)ofs;
00195 }
00196 
00201 banded_lower_triangular_dvar_matrix::banded_lower_triangular_dvar_matrix
00202   (int _min,int _max,int _bw)
00203 {
00204   bw=_bw;
00205   ivector  lb(0,_bw-1);
00206   lb.fill_seqadd(_min,1);
00207   d.allocate(0,_bw-1,lb,_max);
00208 }
00209 
00214 ostream& operator<<(const ostream& _ofs,
00215   const banded_lower_triangular_dvar_matrix& S1)
00216 {
00217   ostream& ofs= (ostream&) _ofs;
00218   banded_lower_triangular_dvar_matrix& S =
00219     (banded_lower_triangular_dvar_matrix&)S1;
00220   int imin=S.indexmin();
00221   int imax=S.indexmax();
00222   int bw=S.bandwidth();
00223   for (int i=imin;i<=imax;i++)
00224   {
00225     for (int j=imin;j<=imax;j++)
00226     {
00227       if (j<=i)
00228       {
00229         if ( (i-j) < bw)
00230           ofs << S(i,j) << " ";
00231         else
00232           ofs << 0.0 << " ";
00233       }
00234       else
00235       {
00236         ofs << 0.0 << " ";
00237       }
00238     }
00239     if (i<imax) ofs << endl;
00240   }
00241   return (ostream&)ofs;
00242 }
00243 
00244 
00245 
00246 #include "fvar.hpp"
00247 
00248 #ifdef __TURBOC__
00249 #pragma hdrstop
00250 #include <iostream.h>
00251 #endif
00252 
00253 #ifdef __ZTC__
00254 #include <iostream.hpp>
00255 #endif
00256 
00257 #ifdef __SUN__
00258 #include <iostream.h>
00259 #endif
00260 #ifdef __NDPX__
00261 #include <iostream.h>
00262 #endif
00263 void dfcholeski_decomp(void);
00264 void dfcholeski_decomp_banded(void);
00265 
00270 dvariable ln_det_choleski(
00271   const banded_symmetric_dvar_matrix& MM, const int& _ierr)
00272 {
00273   banded_lower_triangular_dvar_matrix tmp=choleski_decomp(MM,_ierr);
00274   dvariable ld=0.0;
00275   if (_ierr==1)
00276   {
00277     return ld;
00278   }
00279 
00280   int mmin=tmp.indexmin();
00281   int mmax=tmp.indexmax();
00282   for (int i=mmin;i<=mmax;i++)
00283   {
00284     ld+=log(tmp(i,i));
00285   }
00286   return 2.0*ld;
00287 }
00288 
00293 banded_lower_triangular_dvar_matrix choleski_decomp(
00294   const banded_symmetric_dvar_matrix& MM, const int& _ierr)
00295 {
00296   int& ierr=(int&)(_ierr);
00297   // kludge to deal with constantness
00298   ierr =0;
00299   banded_symmetric_dvar_matrix& M = (banded_symmetric_dvar_matrix&) MM;
00300   int n=M.indexmax();
00301 
00302   int bw=M.bandwidth();
00303   banded_lower_triangular_dvar_matrix L(1,n,bw);
00304 #ifndef SAFE_INITIALIZE
00305     L.initialize();
00306 #endif
00307 
00308   int i,j,k;
00309   double tmp;
00310     if (M(1,1)<=0)
00311     {
00312       cerr << "Error matrix not positive definite in choleski_decomp"
00313        " value was " << M(1,1) << " for index  1"   <<endl;
00314       ierr=1;
00315       M(1,1)=1.0;
00316     }
00317   L.elem_value(1,1)=sqrt(value(M(1,1)));
00318   for (i=2;i<=bw;i++)
00319   {
00320     L.elem_value(i,1)=value(M(i,1))/L.elem_value(1,1);
00321   }
00322 
00323   for (i=2;i<=n;i++)
00324   {
00325     int jmin=admax(2,i-bw+1);
00326     for (j=jmin;j<=i-1;j++)
00327     {
00328       tmp=value(M(i,j));
00329       int kmin=max(1,j-bw+1,i-bw+1);
00330       for (k=kmin;k<=j-1;k++)
00331       {
00332         tmp-=L.elem_value(i,k)*L.elem_value(j,k);
00333       }
00334       L.elem_value(i,j)=tmp/L.elem_value(j,j);
00335     }
00336     tmp=value(M(i,i));
00337     int kmin=admax(i-bw+1,1);
00338     //double vmax=fabs(L.elem_value(i,kmin));
00339     //for (k=kmin;k<=i-1;k++)
00340     //{
00341      // if (fabs(L.elem_value(i,k))>vmax) vmax=fabs(L.elem_value(i,k));
00342     //}
00343     //tmp/=square(vmax);
00344     for (k=kmin;k<=i-1;k++)
00345     {
00346       tmp-=L.elem_value(i,k)*L.elem_value(i,k);
00347       //tmp-=square(L.elem_value(i,k)/vmax);
00348     }
00349     if (tmp<=0)
00350     {
00351       cerr << "Error matrix not positive definite in choleski_decomp"
00352        " value was " << tmp << " for index " << i    <<endl;
00353       ierr=1;
00354 
00355       int print_switch=0;
00356       if (print_switch)
00357       {
00358         dmatrix CMM=dmatrix(value(MM));
00359         ofstream ofs("hh");
00360         {
00361           ofs << setprecision(3) << setscientific() << setw(11)
00362               << CMM << endl<< endl;
00363         }
00364         dvector ev(CMM.indexmin(),CMM.indexmax());
00365         dmatrix evec=eigenvectors(CMM,ev);
00366         ofs << setprecision(3) << setscientific() << setw(11)
00367               << ev << endl<< endl;
00368         ofs << setprecision(3) << setscientific() << setw(11)
00369               << evec << endl<< endl;
00370         uostream uos("uos");
00371         uos << CMM.indexmax()-CMM.indexmin()+1;
00372         uos << CMM;
00373       }
00374       /*
00375       dmatrix N(1,4,1,4);
00376 
00377       for (int i=1;i<=4;i++)
00378       {
00379         N(i,i)=value(M(i,i));
00380         for (int j=1;j<i;j++)
00381         {
00382           N(i,j)=value(M(i,j));
00383           N(j,i)=value(M(i,j));
00384         }
00385       }
00386       cout << N << endl;
00387       cout << eigenvalues(N) << endl;
00388       cout << choleski_decomp(N) << endl;
00389       */
00390 
00391       tmp=1.0;
00392     }
00393     //L.elem_value(i,i)=vmax*sqrt(tmp);
00394     L.elem_value(i,i)=sqrt(tmp);
00395   }
00396 
00397   //banded_lower_triangular_dvar_matrix vc=nograd_assign(L);
00398   save_identifier_string("rs");
00399   L.save_dvar_matrix_position();
00400   save_identifier_string("rt");
00401   MM.save_dvar_matrix_value();
00402   save_identifier_string("rl");
00403   MM.save_dvar_matrix_position();
00404   save_identifier_string("ro");
00405   gradient_structure::GRAD_STACK1->
00406       set_gradient_stack(dfcholeski_decomp_banded);
00407 
00408   return L;
00409 }
00410 
00415 void dfcholeski_decomp_banded(void)
00416 {
00417   verify_identifier_string("ro");
00418   dvar_matrix_position MMpos=restore_dvar_matrix_position();
00419   verify_identifier_string("rl");
00420   banded_symmetric_dmatrix M=
00421     restore_banded_symmetric_dvar_matrix_value(MMpos);
00422   verify_identifier_string("rt");
00423   dvar_matrix_position vcpos=restore_dvar_matrix_position();
00424   verify_identifier_string("rs");
00425   banded_lower_triangular_dmatrix dfL=
00426     restore_banded_lower_triangular_dvar_matrix_derivatives(vcpos);
00427 
00428   int n=M.indexmax();
00429   int bw=M.bandwidth();
00430 
00431   banded_lower_triangular_dmatrix L(1,n,bw);
00432   banded_lower_triangular_dmatrix tmp1(1,n,bw);
00433   banded_lower_triangular_dmatrix dftmp1(1,n,bw);
00434   banded_symmetric_dmatrix dfM(1,n,bw);
00435   dvector tmp(1,n);
00436   dvector dftmp(1,n);
00437   tmp.initialize();
00438   tmp1.initialize();
00439   dftmp.initialize();
00440   dftmp1.initialize();
00441   dfM.initialize();
00442 #ifndef SAFE_INITIALIZE
00443     L.initialize();
00444 #endif
00445 
00446   int i,j,k;
00447   if (M(1,1)<=0)
00448   {
00449     cerr << "Error matrix not positive definite in choleski_decomp"
00450       <<endl;
00451     M(1,1)=1.0;
00452     //ad_exit(1);
00453   }
00454   L(1,1)=sqrt(M(1,1));
00455   for (i=2;i<=bw;i++)
00456   {
00457     L(i,1)=M(i,1)/L(1,1);
00458   }
00459 
00460   for (i=2;i<=n;i++)
00461   {
00462     int jmin=admax(2,i-bw+1);
00463     for (j=jmin;j<=i-1;j++)
00464     {
00465       tmp1(i,j)=M(i,j);
00466       int kmin=max(1,j-bw+1,i-bw+1);
00467       for (k=kmin;k<=j-1;k++)
00468       {
00469         tmp1(i,j)-=L(i,k)*L(j,k);
00470       }
00471       L(i,j)=tmp1(i,j)/L(j,j);
00472     }
00473     tmp(i)=M(i,i);
00474     int kmin=admax(i-bw+1,1);
00475     //double vmax=fabs(L(i,kmin));
00476     //for (k=kmin;k<=i-1;k++)
00477    // {
00478      // if (fabs(L(i,k))>vmax) vmax=fabs(L(i,k));
00479    // }
00480     //tmp(i)/=square(vmax);
00481     for (k=kmin;k<=i-1;k++)
00482     {
00483       tmp(i)-=L(i,k)*L(i,k);
00484       //tmp(i)-=square(L(i,k)/vmax);
00485     }
00486     if (tmp(i)<=0)
00487     {
00488       cerr << "Error matrix not positive definite in choleski_decomp"
00489         <<endl;
00490       tmp(i)=1.0;
00491       //ad_exit(1);
00492     }
00493     //L(i,i)=vmax*sqrt(tmp(i));
00494     L(i,i)=sqrt(tmp(i));
00495   }
00496  //*******************************************************************8
00497  //*******************************************************************8
00498  //*******************************************************************8
00499   for (i=n;i>=2;i--)
00500   {
00501     //L(i,i)=sqrt(tmp(i));
00502     dftmp(i)+=dfL(i,i)/(2.0*L(i,i));
00503     dfL(i,i)=0.0;
00504     int kmin=admax(i-bw+1,1);
00505     for (k=i-1;k>=kmin;k--)
00506     {
00507       //tmp(i)-=L(i,k)*L(i,k);
00508       dfL(i,k)-=2.*dftmp(i)*L(i,k);
00509     }
00510     //tmp(i)=M(i,i);
00511     dfM(i,i)+=dftmp(i);
00512     dftmp(i)=0.0;
00513     int jmin=admax(2,i-bw+1);
00514     for (j=i-1;j>=jmin;j--)
00515     {
00516       //L(i,j)=tmp1(i,j)/L(j,j);
00517       double linv=1./L(j,j);
00518       dftmp1(i,j)+=dfL(i,j)*linv;
00519       dfL(j,j)-=dfL(i,j)*tmp1(i,j)*linv*linv;
00520       dfL(i,j)=0.0;
00521       int kmin=max(1,j-bw+1,i-bw+1);
00522       for (k=j-1;k>=kmin;k--)
00523       {
00524         //tmp(i,j)-=L(i,k)*L(j,k);
00525         dfL(i,k)-=dftmp1(i,j)*L(j,k);
00526         dfL(j,k)-=dftmp1(i,j)*L(i,k);
00527       }
00528       //tmp(i,j)=M(i,j);
00529       dfM(i,j)+=dftmp1(i,j);
00530       dftmp1(i,j)=0.0;
00531     }
00532   }
00533   double linv=1./L(1,1);
00534   for (i=bw;i>=2;i--)
00535   {
00536     //L(i,1)=M(i,1)/L(1,1);
00537     dfM(i,1)+=dfL(i,1)*linv;
00538     dfL(1,1)-=dfL(i,1)*M(i,1)*linv*linv;
00539     dfL(i,1)=0.0;
00540   }
00541   //L(1,1)=sqrt(M(1,1));
00542   dfM(1,1)+=dfL(1,1)/(2.*L(1,1));
00543 
00544 
00545  //*******************************************************************8
00546  //*******************************************************************8
00547  //*******************************************************************8
00548 
00549   dfM.save_dmatrix_derivatives(MMpos);
00550 }
00551 
00556 dvar_matrix::dvar_matrix(const banded_symmetric_dvar_matrix& S1)
00557 {
00558   banded_symmetric_dvar_matrix& S= (banded_symmetric_dvar_matrix&) S1;
00559   int imin=S.indexmin();
00560   int imax=S.indexmax();
00561   int bw=S.bandwidth();
00562   allocate(imin,imax,imin,imax);
00563   int i1;
00564   int j1;
00565   initialize();
00566   for (int i=imin;i<=imax;i++)
00567   {
00568     for (int j=imin;j<=imax;j++)
00569     {
00570       if (j<=i)
00571       {
00572         j1=j;
00573         i1=i;
00574       }
00575       else
00576       {
00577         j1=i;
00578         i1=j;
00579       }
00580       if ( (i1-j1) < bw)
00581         (*this)(i,j)=S(i1,j1);
00582     }
00583   }
00584 }
00585 
00590 dvar_matrix::dvar_matrix(const banded_lower_triangular_dvar_matrix& S1)
00591 {
00592   banded_lower_triangular_dvar_matrix& S=
00593     (banded_lower_triangular_dvar_matrix&) S1;
00594   int imin=S.indexmin();
00595   int imax=S.indexmax();
00596   int bw=S.bandwidth();
00597   allocate(imin,imax,imin,imax);
00598   initialize();
00599   for (int i=imin;i<=imax;i++)
00600   {
00601     for (int j=imin;j<=imax;j++)
00602     {
00603       if (j<=i)
00604       {
00605         if ( (i-j) < bw)
00606           (*this)(i,j)=S(i,j);
00607       }
00608     }
00609   }
00610 }
00611 
00616 int max(int i,int j,int k)
00617 {
00618   if (i>j)
00619     if ( i>k)
00620       return i;
00621     else
00622       return k;
00623   else
00624     if ( j>k)
00625       return j;
00626     else
00627       return k;
00628 }
00629 
00634 dmatrix restore_lower_triangular_dvar_matrix_value(
00635   const dvar_matrix_position& mpos)
00636 {
00637   // restores the size, address, and value information for a dvar_matrix
00638   banded_lower_triangular_dmatrix out((const dvar_matrix_position&)mpos);
00639   //int ierr;
00640   int min=out.rowmin();
00641   int max=out.rowmax();
00642   for (int i=max;i>=min;i--)
00643   {
00644     dvar_vector_position vpos=restore_dvar_vector_position();
00645     out(i)=restore_dvar_vector_value(vpos);
00646   }
00647   return out;
00648 }
00649 
00654 void check_choleski_decomp(const banded_symmetric_dvar_matrix& MM,
00655   const int& _ierr)
00656 {
00657   int& ierr=(int&)(_ierr);
00658   // kludge to deal with constantness
00659   ierr =0;
00660   banded_symmetric_dvar_matrix& M = (banded_symmetric_dvar_matrix&) MM;
00661   int n=M.indexmax();
00662 
00663   int bw=M.bandwidth();
00664   banded_lower_triangular_dvar_matrix L(1,n,bw);
00665 #ifndef SAFE_INITIALIZE
00666     L.initialize();
00667 #endif
00668 
00669   int i,j,k;
00670   double tmp;
00671     if (M(1,1)<=0)
00672     {
00673       cerr << "Error matrix not positive definite in choleski_decomp"
00674        " value was " << M(1,1) << " for index  1"   <<endl;
00675       ierr=1;
00676       return;
00677       M(1,1)=1.0;
00678     }
00679   L.elem_value(1,1)=sqrt(value(M(1,1)));
00680   for (i=2;i<=bw;i++)
00681   {
00682     L.elem_value(i,1)=value(M(i,1))/L.elem_value(1,1);
00683   }
00684 
00685   for (i=2;i<=n;i++)
00686   {
00687     int jmin=admax(2,i-bw+1);
00688     for (j=jmin;j<=i-1;j++)
00689     {
00690       tmp=value(M(i,j));
00691       int kmin=max(1,j-bw+1,i-bw+1);
00692       for (k=kmin;k<=j-1;k++)
00693       {
00694         tmp-=L.elem_value(i,k)*L.elem_value(j,k);
00695       }
00696       L.elem_value(i,j)=tmp/L.elem_value(j,j);
00697     }
00698     tmp=value(M(i,i));
00699     int kmin=admax(i-bw+1,1);
00700     for (k=kmin;k<=i-1;k++)
00701     {
00702       tmp-=L.elem_value(i,k)*L.elem_value(i,k);
00703     }
00704     if (tmp<=0)
00705     {
00706       cerr << "Error matrix not positive definite in choleski_decomp"
00707        " value was " << tmp << " for index " << i    <<endl;
00708       ierr=1;
00709       return;
00710       tmp=1.0;
00711     }
00712     L.elem_value(i,i)=sqrt(tmp);
00713   }
00714 }
00715 
00720 dvariable norm(const banded_symmetric_dvar_matrix& B)
00721 {
00722   return sqrt(norm2(B));
00723 }
00724 
00729 dvariable norm2(const banded_symmetric_dvar_matrix& B)
00730 {
00731   dvariable nm=0.0;
00732   for (int i=1;i<=B.bw-1;i++)
00733   {
00734     nm+=norm2(B.d(i));
00735   }
00736   nm*=2;
00737   nm+=norm2(B.d(0));
00738   return nm;
00739 }
00740 dvariable sumsq(const banded_symmetric_dvar_matrix& B) {return(norm2(B));}