ADMB Documentation  11.1.2192
 All Classes Files Functions Variables Typedefs Friends Defines
fvar_m51.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id: fvar_m51.cpp 1919 2014-04-22 22:02:01Z johnoel $
00003  *
00004  * Author: David Fournier
00005  * Copyright (c) 2008-2012 Regents of the University of California
00006  */
00011 #include "fvar.hpp"
00012 
00013 #ifdef __TURBOC__
00014 #pragma hdrstop
00015 #include <iostream.h>
00016 #endif
00017 
00018 #ifdef __ZTC__
00019 #include <iostream.hpp>
00020 #endif
00021 
00022 #ifdef __SUN__
00023 #include <iostream.h>
00024 #endif
00025 #ifdef __NDPX__
00026 #include <iostream.h>
00027 #endif
00028 void dfcholeski_decomp(void);
00029 
00030 // compute log of determinant of a spd matrix
00031 void df_ln_det_choleski(void);
00032 
00037 dvariable ln_det_choleski(const dvar_matrix& MM)
00038 {
00039   // kludge to deal with constantness
00040   dmatrix M=value(MM);
00041   if (M.colsize() != M.rowsize())
00042   {
00043     cerr << "Error in chol_decomp. Matrix not square" << endl;
00044     ad_exit(1);
00045   }
00046   if (M.colsize() == 1)
00047   {
00048     int mmin=M.indexmin();
00049     return log(MM(mmin,mmin));
00050   }
00051 
00052   //int rowsave=M.rowmin();
00053   //int colsave=M.colmin();
00054   M.rowshift(1);
00055   M.colshift(1);
00056   int n=M.rowmax();
00057 
00058   dmatrix L(1,n,1,n);
00059   //dmatrix C(1,n,1,n);
00060   //imatrix B(1,n,1,n);
00061   //B.initialize();
00062 #ifndef SAFE_INITIALIZE
00063     L.initialize();
00064 #endif
00065 
00066   int i,j,k;
00067   double tmp;
00068     if (M(1,1)<=0)
00069     {
00070       cerr << "Error matrix not positive definite in choleski_decomp"
00071         <<endl;
00072       ad_exit(1);
00073     }
00074   L(1,1)=sqrt(M(1,1));
00075   for (i=2;i<=n;i++)
00076   {
00077     L(i,1)=M(i,1)/L(1,1);
00078   }
00079 
00080   for (i=2;i<=n;i++)
00081   {
00082     for (j=2;j<=i-1;j++)
00083     {
00084       tmp=M(i,j);
00085       for (k=1;k<=j-1;k++)
00086       {
00087         tmp-=L(i,k)*L(j,k);
00088       }
00089       L(i,j)=tmp/L(j,j);
00090     }
00091     tmp=M(i,i);
00092     for (k=1;k<=i-1;k++)
00093     {
00094       tmp-=L(i,k)*L(i,k);
00095     }
00096     if (tmp<=0)
00097     {
00098       cerr << "Error matrix not positive definite in ln_det_choleski"
00099         <<endl;
00100       ad_exit(1);
00101     }
00102     L(i,i)=sqrt(tmp);
00103   }
00104   //L.rowshift(rowsave);
00105   //L.colshift(colsave);
00106   double log_det1=0.0;
00107   for (i=1;i<=k;i++)
00108   {
00109     log_det1+=log(L(i,i));
00110   }
00111  /*
00112   double minL=L(1,1);
00113   for (i=2;i<=k;i++)
00114   {
00115     if (L(i,i)<minL) minL=L(i,i);
00116   }
00117   cout << "min in choleski = " << minL << endl;
00118  */
00119 
00120 
00121   double log_det=2.0*log_det1;
00122   dvariable vlog_det=nograd_assign(log_det);
00123   save_identifier_string("ps");
00124   vlog_det.save_prevariable_position();
00125   save_identifier_string("rt");
00126   MM.save_dvar_matrix_value();
00127   save_identifier_string("pl");
00128   MM.save_dvar_matrix_position();
00129   save_identifier_string("pa");
00130   gradient_structure::GRAD_STACK1->
00131       set_gradient_stack(df_ln_det_choleski);
00132   return vlog_det;
00133 }
00134 
00139 void df_ln_det_choleski(void)
00140 {
00141   verify_identifier_string("pa");
00142   dvar_matrix_position MMpos=restore_dvar_matrix_position();
00143   verify_identifier_string("pl");
00144   dmatrix M=restore_dvar_matrix_value(MMpos);
00145   verify_identifier_string("rt");
00146   //prevariable_position vcpos=restore_prevariable_position();
00147   double dflog_det=restore_prevariable_derivative();
00148   verify_identifier_string("ps");
00149 
00150   if (M.colsize() != M.rowsize())
00151   {
00152     cerr << "Error in chol_decomp. Matrix not square" << endl;
00153     ad_exit(1);
00154   }
00155   int rowsave=M.rowmin();
00156   int colsave=M.colmin();
00157   M.rowshift(1);
00158   M.colshift(1);
00159   int n=M.rowmax();
00160 
00161   dmatrix L(1,n,1,n);
00162   dmatrix dfL(1,n,1,n);
00163   dvector tmp(1,n);
00164   dmatrix tmp1(1,n,1,n);
00165   dmatrix dftmp1(1,n,1,n);
00166   dmatrix dfM(1,n,1,n);
00167   dvector dftmp(1,n);
00168   tmp.initialize();
00169   tmp1.initialize();
00170   dftmp.initialize();
00171   dftmp1.initialize();
00172   dfM.initialize();
00173   dfL.initialize();
00174 #ifndef SAFE_INITIALIZE
00175     L.initialize();
00176 #endif
00177 
00178   int i,j,k;
00179   if (M(1,1)<=0)
00180   {
00181     cerr << "Error matrix not positive definite in choleski_decomp"
00182       <<endl;
00183     ad_exit(1);
00184   }
00185   L(1,1)=sqrt(M(1,1));
00186   for (i=2;i<=n;i++)
00187   {
00188     L(i,1)=M(i,1)/L(1,1);
00189   }
00190 
00191   for (i=2;i<=n;i++)
00192   {
00193     for (j=2;j<=i-1;j++)
00194     {
00195       tmp1(i,j)=M(i,j);
00196       for (k=1;k<=j-1;k++)
00197       {
00198         tmp1(i,j)-=L(i,k)*L(j,k);
00199       }
00200       L(i,j)=tmp1(i,j)/L(j,j);
00201     }
00202     tmp(i)=M(i,i);
00203     for (k=1;k<=i-1;k++)
00204     {
00205       tmp(i)-=L(i,k)*L(i,k);
00206     }
00207     if (tmp(i)<=0)
00208     {
00209       cerr << "Error matrix not positive definite in choleski_decomp"
00210         <<endl;
00211       ad_exit(1);
00212     }
00213     L(i,i)=sqrt(tmp(i));
00214   }
00215   double log_det1=0.0;
00216   for (i=1;i<=n;i++)
00217   {
00218     log_det1+=log(L(i,i));
00219   }
00220   //double log_det=2.0*log_det1;
00221 
00222  //*******************************************************************8
00223  //*******************************************************************8
00224  //*******************************************************************8
00225 
00226   //double log_det=2.0*log_det1;
00227   double dflog_det1=2.0*dflog_det;
00228   for (i=1;i<=n;i++)
00229   {
00230     //log_det1+=log(L(i,i));
00231     dfL(i,i)=dflog_det1/L(i,i);
00232   }
00233 
00234   for (i=n;i>=2;i--)
00235   {
00236     //L(i,i)=sqrt(tmp(i));
00237     dftmp(i)+=dfL(i,i)/(2.0*L(i,i));
00238     dfL(i,i)=0.0;
00239     for (k=i-1;k>=1;k--)
00240     {
00241       //tmp(i)-=L(i,k)*L(i,k);
00242       dfL(i,k)-=2.*dftmp(i)*L(i,k);
00243     }
00244     //tmp(i)=M(i,i);
00245     dfM(i,i)+=dftmp(i);
00246     dftmp(i)=0.0;
00247     for (j=i-1;j>=2;j--)
00248     {
00249       //L(i,j)=tmp1(i,j)/L(j,j);
00250       double linv=1./L(j,j);
00251       dftmp1(i,j)+=dfL(i,j)*linv;
00252       dfL(j,j)-=dfL(i,j)*tmp1(i,j)*linv*linv;
00253       dfL(i,j)=0.0;
00254       for (k=j-1;k>=1;k--)
00255       {
00256         //tmp(i,j)-=L(i,k)*L(j,k);
00257         dfL(i,k)-=dftmp1(i,j)*L(j,k);
00258         dfL(j,k)-=dftmp1(i,j)*L(i,k);
00259       }
00260       //tmp(i,j)=M(i,j);
00261       dfM(i,j)+=dftmp1(i,j);
00262       dftmp1(i,j)=0.0;
00263     }
00264   }
00265   double linv=1./L(1,1);
00266   for (i=n;i>=2;i--)
00267   {
00268     //L(i,1)=M(i,1)/L(1,1);
00269     dfM(i,1)+=dfL(i,1)*linv;
00270     dfL(1,1)-=dfL(i,1)*M(i,1)*linv*linv;
00271     dfL(i,1)=0.0;
00272   }
00273   //L(1,1)=sqrt(M(1,1));
00274   dfM(1,1)+=dfL(1,1)/(2.*L(1,1));
00275 
00276 
00277  //*******************************************************************8
00278  //*******************************************************************8
00279  //*******************************************************************8
00280 
00281   dfM.rowshift(rowsave);
00282   dfM.colshift(colsave);
00283 
00284   dfM.save_dmatrix_derivatives(MMpos);
00285 }
00286 
00291 static dvariable error_condition(int &onerror)
00292 {
00293   onerror=1;
00294   dvariable v=0.0;
00295   return v;
00296 }
00297 
00302 dvariable ln_det_choleski_error(const dvar_matrix& MM,int & onerror)
00303 {
00304   onerror=0;
00305   // kludge to deal with constantness
00306   dmatrix M=value(MM);
00307   if (M.colsize() != M.rowsize())
00308   {
00309     cerr << "Error in chol_decomp. Matrix not square" << endl;
00310     ad_exit(1);
00311   }
00312   if (M.colsize() == 1)
00313   {
00314     int mmin=M.indexmin();
00315     return log(MM(mmin,mmin));
00316   }
00317 
00318   //int rowsave=M.rowmin();
00319   //int colsave=M.colmin();
00320   M.rowshift(1);
00321   M.colshift(1);
00322   int n=M.rowmax();
00323 
00324   dmatrix L(1,n,1,n);
00325   //dmatrix C(1,n,1,n);
00326   //imatrix B(1,n,1,n);
00327   //B.initialize();
00328 #ifndef SAFE_INITIALIZE
00329     L.initialize();
00330 #endif
00331 
00332   int i,j,k;
00333   double tmp;
00334     if (M(1,1)<=0)
00335     {
00336       return error_condition(onerror);
00337     }
00338   L(1,1)=sqrt(M(1,1));
00339   for (i=2;i<=n;i++)
00340   {
00341     L(i,1)=M(i,1)/L(1,1);
00342   }
00343 
00344   for (i=2;i<=n;i++)
00345   {
00346     for (j=2;j<=i-1;j++)
00347     {
00348       tmp=M(i,j);
00349       for (k=1;k<=j-1;k++)
00350       {
00351         tmp-=L(i,k)*L(j,k);
00352       }
00353       L(i,j)=tmp/L(j,j);
00354     }
00355     tmp=M(i,i);
00356     for (k=1;k<=i-1;k++)
00357     {
00358       tmp-=L(i,k)*L(i,k);
00359     }
00360     if (tmp<=0)
00361     {
00362       return error_condition(onerror);
00363     }
00364     L(i,i)=sqrt(tmp);
00365   }
00366   //L.rowshift(rowsave);
00367   //L.colshift(colsave);
00368   double log_det1=0.0;
00369   for (i=1;i<=k;i++)
00370   {
00371     log_det1+=log(L(i,i));
00372   }
00373  /*
00374   double minL=L(1,1);
00375   for (i=2;i<=k;i++)
00376   {
00377     if (L(i,i)<minL) minL=L(i,i);
00378   }
00379   cout << "min in choleski = " << minL << endl;
00380  */
00381 
00382 
00383   double log_det=2.0*log_det1;
00384   dvariable vlog_det=nograd_assign(log_det);
00385   save_identifier_string("ps");
00386   vlog_det.save_prevariable_position();
00387   save_identifier_string("rt");
00388   MM.save_dvar_matrix_value();
00389   save_identifier_string("pl");
00390   MM.save_dvar_matrix_position();
00391   save_identifier_string("pa");
00392   gradient_structure::GRAD_STACK1->
00393       set_gradient_stack(df_ln_det_choleski);
00394   return vlog_det;
00395 }