ADMB Documentation  11.1x.2723
 All Classes Files Functions Variables Typedefs Friends Defines
fvar_m51.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id: fvar_m51.cpp 2257 2014-08-27 22:33:53Z 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     if (M(1,1)<=0)
00067     {
00068       cerr << "Error matrix not positive definite in choleski_decomp"
00069         <<endl;
00070       ad_exit(1);
00071     }
00072   L(1,1)=sqrt(M(1,1));
00073   for (int i=2;i<=n;i++)
00074   {
00075     L(i,1)=M(i,1)/L(1,1);
00076   }
00077 
00078   double tmp;
00079   for (int i=2;i<=n;i++)
00080   {
00081     for (int j=2;j<=i-1;j++)
00082     {
00083       tmp=M(i,j);
00084       for (int k=1;k<=j-1;k++)
00085       {
00086         tmp-=L(i,k)*L(j,k);
00087       }
00088       L(i,j)=tmp/L(j,j);
00089     }
00090     tmp=M(i,i);
00091     for (int k=1;k<=i-1;k++)
00092     {
00093       tmp-=L(i,k)*L(i,k);
00094     }
00095     if (tmp<=0)
00096     {
00097       cerr << "Error matrix not positive definite in ln_det_choleski"
00098         <<endl;
00099       ad_exit(1);
00100     }
00101     L(i,i)=sqrt(tmp);
00102   }
00103   //L.rowshift(rowsave);
00104   //L.colshift(colsave);
00105   double log_det1=0.0;
00106   for (int i=1;i<=n;i++)
00107   {
00108     log_det1+=log(L(i,i));
00109   }
00110  /*
00111   double minL=L(1,1);
00112   for (i=2;i<=k;i++)
00113   {
00114     if (L(i,i)<minL) minL=L(i,i);
00115   }
00116   cout << "min in choleski = " << minL << endl;
00117  */
00118 
00119 
00120   double log_det=2.0*log_det1;
00121   dvariable vlog_det=nograd_assign(log_det);
00122   save_identifier_string("ps");
00123   vlog_det.save_prevariable_position();
00124   save_identifier_string("rt");
00125   MM.save_dvar_matrix_value();
00126   save_identifier_string("pl");
00127   MM.save_dvar_matrix_position();
00128   save_identifier_string("pa");
00129   gradient_structure::GRAD_STACK1->
00130       set_gradient_stack(df_ln_det_choleski);
00131   return vlog_det;
00132 }
00133 
00138 void df_ln_det_choleski(void)
00139 {
00140   verify_identifier_string("pa");
00141   dvar_matrix_position MMpos=restore_dvar_matrix_position();
00142   verify_identifier_string("pl");
00143   dmatrix M=restore_dvar_matrix_value(MMpos);
00144   verify_identifier_string("rt");
00145   //prevariable_position vcpos=restore_prevariable_position();
00146   double dflog_det=restore_prevariable_derivative();
00147   verify_identifier_string("ps");
00148 
00149   if (M.colsize() != M.rowsize())
00150   {
00151     cerr << "Error in chol_decomp. Matrix not square" << endl;
00152     ad_exit(1);
00153   }
00154   int rowsave=M.rowmin();
00155   int colsave=M.colmin();
00156   M.rowshift(1);
00157   M.colshift(1);
00158   int n=M.rowmax();
00159 
00160   dmatrix L(1,n,1,n);
00161   dmatrix dfL(1,n,1,n);
00162   dvector tmp(1,n);
00163   dmatrix tmp1(1,n,1,n);
00164   dmatrix dftmp1(1,n,1,n);
00165   dmatrix dfM(1,n,1,n);
00166   dvector dftmp(1,n);
00167   tmp.initialize();
00168   tmp1.initialize();
00169   dftmp.initialize();
00170   dftmp1.initialize();
00171   dfM.initialize();
00172   dfL.initialize();
00173 #ifndef SAFE_INITIALIZE
00174     L.initialize();
00175 #endif
00176 
00177   if (M(1,1)<=0)
00178   {
00179     cerr << "Error matrix not positive definite in choleski_decomp"
00180       <<endl;
00181     ad_exit(1);
00182   }
00183   L(1,1)=sqrt(M(1,1));
00184   for (int i=2;i<=n;i++)
00185   {
00186     L(i,1)=M(i,1)/L(1,1);
00187   }
00188 
00189   for (int i=2;i<=n;i++)
00190   {
00191     for (int j=2;j<=i-1;j++)
00192     {
00193       tmp1(i,j)=M(i,j);
00194       for (int k=1;k<=j-1;k++)
00195       {
00196         tmp1(i,j)-=L(i,k)*L(j,k);
00197       }
00198       L(i,j)=tmp1(i,j)/L(j,j);
00199     }
00200     tmp(i)=M(i,i);
00201     for (int k=1;k<=i-1;k++)
00202     {
00203       tmp(i)-=L(i,k)*L(i,k);
00204     }
00205     if (tmp(i)<=0)
00206     {
00207       cerr << "Error matrix not positive definite in choleski_decomp"
00208         <<endl;
00209       ad_exit(1);
00210     }
00211     L(i,i)=sqrt(tmp(i));
00212   }
00213   double log_det1=0.0;
00214   for (int i=1;i<=n;i++)
00215   {
00216     log_det1+=log(L(i,i));
00217   }
00218   //double log_det=2.0*log_det1;
00219 
00220  //*******************************************************************8
00221  //*******************************************************************8
00222  //*******************************************************************8
00223 
00224   //double log_det=2.0*log_det1;
00225   double dflog_det1=2.0*dflog_det;
00226   for (int i=1;i<=n;i++)
00227   {
00228     //log_det1+=log(L(i,i));
00229     dfL(i,i)=dflog_det1/L(i,i);
00230   }
00231 
00232   for (int i=n;i>=2;i--)
00233   {
00234     //L(i,i)=sqrt(tmp(i));
00235     dftmp(i)+=dfL(i,i)/(2.0*L(i,i));
00236     dfL(i,i)=0.0;
00237     for (int k=i-1;k>=1;k--)
00238     {
00239       //tmp(i)-=L(i,k)*L(i,k);
00240       dfL(i,k)-=2.*dftmp(i)*L(i,k);
00241     }
00242     //tmp(i)=M(i,i);
00243     dfM(i,i)+=dftmp(i);
00244     dftmp(i)=0.0;
00245     for (int j=i-1;j>=2;j--)
00246     {
00247       //L(i,j)=tmp1(i,j)/L(j,j);
00248       double linv=1./L(j,j);
00249       dftmp1(i,j)+=dfL(i,j)*linv;
00250       dfL(j,j)-=dfL(i,j)*tmp1(i,j)*linv*linv;
00251       dfL(i,j)=0.0;
00252       for (int k=j-1;k>=1;k--)
00253       {
00254         //tmp(i,j)-=L(i,k)*L(j,k);
00255         dfL(i,k)-=dftmp1(i,j)*L(j,k);
00256         dfL(j,k)-=dftmp1(i,j)*L(i,k);
00257       }
00258       //tmp(i,j)=M(i,j);
00259       dfM(i,j)+=dftmp1(i,j);
00260       dftmp1(i,j)=0.0;
00261     }
00262   }
00263   double linv=1./L(1,1);
00264   for (int i=n;i>=2;i--)
00265   {
00266     //L(i,1)=M(i,1)/L(1,1);
00267     dfM(i,1)+=dfL(i,1)*linv;
00268     dfL(1,1)-=dfL(i,1)*M(i,1)*linv*linv;
00269     dfL(i,1)=0.0;
00270   }
00271   //L(1,1)=sqrt(M(1,1));
00272   dfM(1,1)+=dfL(1,1)/(2.*L(1,1));
00273 
00274 
00275  //*******************************************************************8
00276  //*******************************************************************8
00277  //*******************************************************************8
00278 
00279   dfM.rowshift(rowsave);
00280   dfM.colshift(colsave);
00281 
00282   dfM.save_dmatrix_derivatives(MMpos);
00283 }
00284 
00289 static dvariable error_condition(int &onerror)
00290 {
00291   onerror=1;
00292   dvariable v=0.0;
00293   return v;
00294 }
00295 
00300 dvariable ln_det_choleski_error(const dvar_matrix& MM,int & onerror)
00301 {
00302   onerror=0;
00303   // kludge to deal with constantness
00304   dmatrix M=value(MM);
00305   if (M.colsize() != M.rowsize())
00306   {
00307     cerr << "Error in chol_decomp. Matrix not square" << endl;
00308     ad_exit(1);
00309   }
00310   if (M.colsize() == 1)
00311   {
00312     int mmin=M.indexmin();
00313     return log(MM(mmin,mmin));
00314   }
00315 
00316   //int rowsave=M.rowmin();
00317   //int colsave=M.colmin();
00318   M.rowshift(1);
00319   M.colshift(1);
00320   int n=M.rowmax();
00321 
00322   dmatrix L(1,n,1,n);
00323   //dmatrix C(1,n,1,n);
00324   //imatrix B(1,n,1,n);
00325   //B.initialize();
00326 #ifndef SAFE_INITIALIZE
00327     L.initialize();
00328 #endif
00329 
00330     if (M(1,1)<=0)
00331     {
00332       return error_condition(onerror);
00333     }
00334   L(1,1)=sqrt(M(1,1));
00335   for (int i=2;i<=n;i++)
00336   {
00337     L(i,1)=M(i,1)/L(1,1);
00338   }
00339 
00340   double tmp;
00341   for (int i=2;i<=n;i++)
00342   {
00343     for (int j=2;j<=i-1;j++)
00344     {
00345       tmp=M(i,j);
00346       for (int k=1;k<=j-1;k++)
00347       {
00348         tmp-=L(i,k)*L(j,k);
00349       }
00350       L(i,j)=tmp/L(j,j);
00351     }
00352     tmp=M(i,i);
00353     for (int k=1;k<=i-1;k++)
00354     {
00355       tmp-=L(i,k)*L(i,k);
00356     }
00357     if (tmp<=0)
00358     {
00359       return error_condition(onerror);
00360     }
00361     L(i,i)=sqrt(tmp);
00362   }
00363   //L.rowshift(rowsave);
00364   //L.colshift(colsave);
00365   double log_det1=0.0;
00366   for (int i=1;i<=n;i++)
00367   {
00368     log_det1+=log(L(i,i));
00369   }
00370  /*
00371   double minL=L(1,1);
00372   for (i=2;i<=k;i++)
00373   {
00374     if (L(i,i)<minL) minL=L(i,i);
00375   }
00376   cout << "min in choleski = " << minL << endl;
00377  */
00378   double log_det=2.0*log_det1;
00379   dvariable vlog_det=nograd_assign(log_det);
00380   save_identifier_string("ps");
00381   vlog_det.save_prevariable_position();
00382   save_identifier_string("rt");
00383   MM.save_dvar_matrix_value();
00384   save_identifier_string("pl");
00385   MM.save_dvar_matrix_position();
00386   save_identifier_string("pa");
00387   gradient_structure::GRAD_STACK1->
00388       set_gradient_stack(df_ln_det_choleski);
00389   return vlog_det;
00390 }