OLINALG.CPP Source File Main Page Namespace List Class Hierarchy Alphabetical List Compound List File List Namespace Members Compound Members File Members
OLINALG.CPP

Go to the documentation of this file.
00001 //Filename    : OLinAlg.cpp
00002 //Description : LinAlg Class Definition:LinearAlgebra
00003 //Owner       : Fred
00004 
00005 // ES library header file
00006 #include <All.h>                                  // err*
00007 
00008 // CU header file
00009 
00010 #include "OLinAlg.h"
00011 
00012 //#define DEBUG_VC
00013 
00014 #ifdef DEBUG_VC
00015 #include <STDIO.H>                                //temp for debug matrix
00016 #endif
00017 #include <OLOG.H>                                 // for debug matrix
00018 
00019 #ifdef DEBUG_CONSOLE
00020 #include <STDIO.H>
00021 #define PRT_MAT (cout <<setw(10)<<setprecision(5))
00022 #define PRT_MAT15 (cout <<setw(10)<<setprecision(15))
00023 #else
00024 #define PRT_MAT
00025 #define PRT_MAT15
00026 #endif
00027 
00028 #define fabs(x) ((x)>0 ? (x):-(x))
00029 
00030 //----------- Define constants -------------//
00031 //----------- Define static variables -------------//
00032 //----------- Define static functions -------------//
00033 //----------- Define static class member variables -------------//
00034 
00035 /*
00036 //----------- Begin of function LinAlg Constructor -----//
00041 LinearAlgebra::LinearAlgebra()
00042 {
00043 
00044 }
00045 //------------- End of function LinAlg Constructor -----//
00046 
00047 //----------- Begin of function LinAlg Destructor ------//
00049 LinearAlgebra::~LinearAlgebra()
00050 {
00051 
00052 }
00053 //------------- End of function LinAlg Destructor ------//
00054 
00055 //----------- Begin of function LinAlg::init --------//
00057 void LinearAlgebra::init()
00058 {
00059 }
00060 //------------- End of function LinAlg::init --------//
00061 */
00062 
00063 //----------- Begin of function LinAlg::deinit --------//
00065 bool LinearAlgebra::quadratic_prog_not_converged(const Vector &v_x, const Vector &v_y, const Vector &v_Road, const Vector &v_Sigma, REAL gamma) {
00066     //const int MAX                     = 100000;
00067     //const     REAL EPSILON    = 1.0/(10000000000);            // 10^-10
00068     const int MAX     = 1000000;
00069     const REAL EPSILON  = 1.0/(1000000);            // 10^-10
00070 
00071     if ( v_x.MaximumValue() > MAX ) {
00072 #ifdef DEBUG_CONSOLE
00073         cout << ("LinAlg: Problem is primal unbounded:");
00074 #endif
00075 
00076         err_here();
00077     }
00078 
00079     if ( v_y.MaximumValue() > MAX ) {
00080 #ifdef DEBUG_CONSOLE
00081         cout << ("LinAlg: Problem is dual unbounded");
00082 #endif
00083         err_here();
00084     }
00085 
00086     if ( v_Road.Sum() < EPSILON && v_Sigma.Sum() < EPSILON
00087          && gamma < EPSILON
00088         )
00089         return false;
00090     else
00091         return true;
00092 }
00093 
00094 //------------- End of function LinAlg::deinit --------//
00095 
00096 //----------- Begin of function LinAlg::quadratic_prog ------//
00107 bool LinearAlgebra::quadratic_prog(const Vector &v_c, const Matrix &m_Q, const Matrix &m_A, const Vector &v_b, Vector &v_xNames, int loopCountMultiplier) {
00108     // init local variables
00109     int maxIteration = 20;
00110     const REAL SIGMA    = 1/15.0;                   // 1/10.0;
00111     const REAL R      = 9.0/10;
00112     int iter    = 0;
00113 
00114     int n = v_c.Storage();
00115     int m = v_b.Storage();
00116 
00117     maxIteration *= loopCountMultiplier;
00118 
00119 #ifdef DEBUG_VC
00120     DEBUG_LOG("----------- quadratic_prog begin -----------");
00121     // print_input(c,Q,A,b,xNames)
00122     if ( n==10 && m==12 ) {
00123         char s[500];
00124 
00125         DEBUG_LOG("c = ");
00126         sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };",
00127                 v_c(1),v_c(2),v_c(3),
00128                 v_c(4),v_c(5),v_c(6),
00129                 v_c(7),v_c(8),v_c(9), v_c(10));
00130         DEBUG_LOG(s);
00131         DEBUG_LOG("Q = ");
00132         for (int i=1; i<=n; i++) {
00133             sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f},",
00134                     m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9),m_Q(i,10)
00135                 );
00136             DEBUG_LOG(s);
00137         }
00138         DEBUG_LOG("A = ");
00139         for (i=1; i<=m; i++) {
00140             sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f},",
00141                     m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9),m_A(i,10)
00142                 );
00143             DEBUG_LOG(s);
00144         }
00145         DEBUG_LOG("b = ");
00146         sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f,",
00147                 v_b(1),v_b(2),v_b(3),
00148                 v_b(4),v_b(5),v_b(6),
00149                 v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12)
00150             );
00151         DEBUG_LOG(s);
00152         /*sprintf(s, "%f, %f, %f, %f, %f, %f, %f, %f, %f,",
00153           v_b(10),v_b(11),v_b(12),
00154           v_b(13),v_b(14),v_b(15),
00155           v_b(16),v_b(17),v_b(18)
00156           );
00157           DEBUG_LOG(s);
00158           sprintf(s, "%f, %f};",
00159           v_b(19),v_b(20));
00160           DEBUG_LOG(s);*/
00161 
00162     }
00163     else if ( n==9 && m==20 ) {                     // stage 1
00164         char s[500];
00165 
00166         DEBUG_LOG("c = ");
00167         sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };",
00168                 v_c(1),v_c(2),v_c(3),
00169                 v_c(4),v_c(5),v_c(6),
00170                 v_c(7),v_c(8),v_c(9));
00171         DEBUG_LOG(s);
00172         DEBUG_LOG("Q = ");
00173         for (int i=1; i<=n; i++) {
00174             sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, },",
00175                     m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9)
00176                 );
00177             DEBUG_LOG(s);
00178         }
00179         DEBUG_LOG("A = ");
00180         for (i=1; i<=m; i++) {
00181             sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f },",
00182                     m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9)
00183                 );
00184             DEBUG_LOG(s);
00185         }
00186         DEBUG_LOG("b = ");
00187         sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, }, ",
00188                 v_b(1),v_b(2),v_b(3),
00189                 v_b(4),v_b(5),v_b(6),
00190                 v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12),
00191                 v_b(13),v_b(14),v_b(15), v_b(16), v_b(17), v_b(18), v_b(19), v_b(20)
00192             );
00193         DEBUG_LOG(s);
00194     }
00195     else if ( n==10 && m==22 ) {                    // stage 2
00196         char s[500];
00197 
00198         DEBUG_LOG("c = ");
00199         sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };",
00200                 v_c(1),v_c(2),v_c(3),
00201                 v_c(4),v_c(5),v_c(6),
00202                 v_c(7),v_c(8),v_c(9),v_c(10));
00203         DEBUG_LOG(s);
00204         DEBUG_LOG("Q = ");
00205         for (int i=1; i<=n; i++) {
00206             sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, },",
00207                     m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9),m_Q(i,10)
00208                 );
00209             DEBUG_LOG(s);
00210         }
00211         DEBUG_LOG("A = ");
00212         for (i=1; i<=m; i++) {
00213             sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f },",
00214                     m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9),m_A(i,10)
00215                 );
00216             DEBUG_LOG(s);
00217         }
00218         DEBUG_LOG("b = ");
00219         sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, }, ",
00220                 v_b(1),v_b(2),v_b(3),
00221                 v_b(4),v_b(5),v_b(6),
00222                 v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12),
00223                 v_b(13),v_b(14),v_b(15), v_b(16), v_b(17), v_b(18), v_b(19), v_b(20), v_b(21), v_b(22)
00224             );
00225         DEBUG_LOG(s);
00226     }
00227 #endif
00228 
00229     Vector  v_eN(n);    v_eN = 1;
00230     Vector  v_eM(m);    v_eM = 1;
00231 
00232     Vector  v_x(n);   v_x = 1;
00233     Vector  v_z(n);   v_z = 1;
00234     Vector  v_y(m);   v_y = 1;
00235     Vector  v_w(m);   v_w = 1;
00236 
00237     // check  m_A is nxm, m_Q is nxn
00238     err_when(m_A.Nrows() != m || m_A.Ncols() != n );
00239     err_when(m_Q.Nrows() != n || m_Q.Ncols() != n );
00240 
00241     // init for while loop not_converged() checking
00242     //
00243     Vector v_Road = v_b - m_A*(v_x) + v_w;
00244     Vector v_Sigma = v_c - m_A.t() * v_y - v_z + m_Q*v_x;
00245     REAL  gamma   = (v_z.t()*v_x + v_y.t()*v_w).AsScalar();
00246     REAL  mute    = SIGMA*gamma / (n+m);
00247 
00248     DiagonalMatrix X(n),  Xinv(n);
00249     DiagonalMatrix Y(m),  Yinv(m);
00250     DiagonalMatrix Z(n);
00251     DiagonalMatrix W(m);
00252     Matrix T1(n,n), T2(n,m), T3(m,n), T4(m,m);
00253 
00254     Vector KKFvec(n+m);                             //, v_tmp1(n), v_tmp2(m);
00255 
00256     Vector m_temp(m+n);
00257     Vector v_dx(n), v_dy(m), v_dz(n), v_dw(m);
00258     REAL  theeta;
00259 
00260     X=0; Xinv=0; Z=0;
00261     Y=0; Yinv=0; W=0; {
00262 
00263         //      Matrix _t1(n,n), _t2(n,n);              _t1=1;  _t2=2;
00264         //      _t1 = SP(_t1,_t2);
00265     }
00266 
00267     Matrix KKFmat(m+n,m+n);
00268     Matrix KKFmatInverse(m+n,m+n);
00269 
00270     REAL min=1;
00271 
00272     while ( quadratic_prog_not_converged(v_x,v_y,v_Road,v_Sigma,gamma) && iter <= maxIteration && min > 0.0000000001 ) {
00273         iter++;
00274         v_Road  = v_b - m_A*v_x + v_w;
00275         v_Sigma = v_c - m_A.t()*v_y - v_z + m_Q*v_x;
00276         gamma   = (v_z.t()*v_x + v_y.t()*v_w).AsScalar();
00277         mute    = SIGMA*gamma / (n+m);
00278 
00279         X.set_diagonal(v_x);
00280         Y.set_diagonal(v_y);
00281         Z.set_diagonal(v_z);
00282         W.set_diagonal(v_w);
00283 
00284         Xinv.set_diagonal(ElmDivide(v_eN, v_x));
00285         Yinv.set_diagonal(ElmDivide(v_eM, v_y));
00286 
00287         T1 = -(Xinv * Z + m_Q);
00288         T2 = m_A.t();
00289         T3 = m_A;
00290         T4 = Yinv*W;
00291         //PRT_MAT << "T1: " << T1 << endl;
00292         KKFmat = (T1|T2) & (T3|T4);
00293 
00294         KKFvec = (v_c - T2*v_y - mute*Xinv*v_eN + m_Q*v_x)
00295             & (v_b - m_A*v_x + mute*Yinv*v_eM);
00296 
00297         Try {
00298             /*{
00299               bool _f;
00300               DiagonalMatrix _dmat(n+m);                _dmat=1;
00301               Matrix _inv(n+m,n+m);
00302 
00303               SVD(KKFmat, _dmat, _inv);
00304 
00305               _dmat=1;
00306               if ( KKFmat*_inv == _dmat )
00307               _f = true;
00308               else
00309               _f = false;
00310               }*/
00311 
00312             min = fabs(KKFmat(1,1));
00313 
00314             for (int i=2; i<=m+n; i++) {
00315                 if ( min > fabs(KKFmat(i,i)) )            // check diagonal element
00316                     min = fabs(KKFmat(i,i));
00317             }
00318 
00319             KKFmatInverse = KKFmat.i();
00320 
00321             /*
00322               if ( KKFmat.LogDeterminant().Value() )
00323               KKFmatInverse = KKFmat.i();
00324               else
00325               break;
00326             */
00327 
00328             m_temp =  KKFmatInverse * KKFvec;
00329 
00330             v_dx  = m_temp.Rows(1,n);
00331             v_dy  = m_temp.Rows(n+1,n+m);
00332 
00333             v_dz  = Xinv*(mute*v_eN - X*Z*v_eN - Z*v_dx);
00334             v_dw  = Yinv*(mute*v_eM - Y*W*v_eM - W*v_dy);
00335             //PRT_MAT << "v_dx,z,y,w: " << (v_dx|v_dz) <<", "<< (v_dy|v_dw) <<endl;
00336             //PRT_MAT << "v_x: " << v_x << endl;
00337             //PRT_MAT << "ElmDivide(v_x,v_dx): " << ElmDivide(v_x,v_dx) << endl;
00338 
00339             theeta =
00340                 (R/((
00341                     ElmDivide(-v_dx,v_x)
00342                     & ElmDivide(-v_dw,v_w)
00343                     & ElmDivide(-v_dy,v_y)
00344                     & ElmDivide(-v_dz,v_z)
00345                     )).MaximumValue()
00346                     );
00347             if(theeta>1)                                // theeta = min(theeta,1)
00348                 theeta = 1;
00349 
00350             v_x += theeta * v_dx;
00351             v_y += theeta * v_dy;
00352             v_w += theeta * v_dw;
00353             v_z += theeta * v_dz;
00354 
00355 #ifdef DEBUG_CONSOLE
00356             cout << "#iter "<< iter << ": " << v_Road.Sum() << ", " << v_Sigma.Sum() << ", " << gamma << endl;
00357             cout << "Ro:" << v_Road << endl;
00358             PRT_MAT15 << "v_x:" << v_x << endl;
00359             PRT_MAT15 << "v_y:" << v_y << endl;
00360             PRT_MAT15 << "v_w:" << v_w << endl;
00361             PRT_MAT15 << "v_z:" << v_z << endl;
00362 #elif defined(DEBUG_VC)
00363 
00364             char s[200];
00365             sprintf(s, "#iter %d: %f, %f, %f",
00366                     iter, v_Road.Sum(), v_Sigma.Sum(), gamma);
00367             DEBUG_LOG(s);
00368             if ( n == 9 && false ) {
00369                 DEBUG_LOG("x = ");
00370                 sprintf(s, "   %f, %f, %f, %f, %f, %f, %f, %f, %f",
00371                         v_x(1),v_x(2),v_x(3),
00372                         v_x(4),v_x(5),v_x(6),
00373                         v_x(7),v_x(8),v_x(9));
00374                 DEBUG_LOG(s);
00375             }
00376 #endif
00377         }
00378         CatchAll {
00379 #ifdef DEBUG_CONSOLE
00380             cout << Exception::what();
00381 #endif
00382 #ifdef DEBUG_VC
00383             DEBUG_LOG("olinalg: failure in quad_prog");
00384             DEBUG_LOG((char *)Exception::what());
00385 #endif
00386             return false;
00387         }
00388     }                                               // while
00389 
00390     v_xNames = v_x;
00391 
00392     if ( min < 0.00001 ) {                          // 1214 || KKFmat.LogDeterminant().Value() == 0 )
00393         //char s[200];
00394 
00395         //sprintf(s, "#iter %d: %f, %f, %f",
00396         //      iter, v_Road.Sum(), v_Sigma.Sum(), gamma);
00397         //DEBUG_LOG(s);
00398         DEBUG_LOG("--- quad_prog early exit for min < 0.00001 ---");
00399     }
00400     else
00401         DEBUG_LOG("----------- quad_prog normal exit -----------");
00402 
00403 #ifdef DEBUG_CONSOLE
00404     cout << "#iter: " << iter;
00405 #endif
00406 
00407     return true;
00408 }
00409 
00410 //------------- End of function LinAlg::quadratic_prog ------//
Generated on Fri Aug 23 01:38:03 2002 for VirtualU by
1.2.17