Changeset 1218

Timestamp:

10/19/10 22:38:37 (14 years ago)

Author:

vahalam

Message:

inv() function replaced by own ldutg() one based on Gauss elimination

Files:

• library/bdm/design/lq_ctrl.h (modified) (4 diffs)

library/bdm/design/lq_ctrl.h

@@ -4 +4 @@
 
 const bool STRICT_RV = true; //empty RV NOT allowed
+
+//! left matrix division with upper triangular matrix using Gauss elimination
+//! fast variant of inv(A) * B where A is UT matrix, returns result in C and true if no error
+bool ldutg(mat &utA, mat &mB, mat &mC){
+        int utsize = utA.rows();
+        
+        if(utsize != utA.cols()) return false; //utA not square
+        if(utsize != mB.rows()) return false; //incorrect mB size
+        
+        int mbcol = mB.cols();
+        
+        mC.set_size(utsize, mbcol);
+        
+        int totsize = utsize + mbcol;
+        int sqs = utsize*utsize;
+        int i, j, k;
+        
+        double pvt;
+        
+        double tmp[utsize*totsize];
+        
+        //fill tmp array
+        for(i = 0; i < sqs; i++) 
+                tmp[i] = utA._data()[i];
+        for(i = sqs; i < utsize*totsize; i++)                           
+                tmp[i] = mB._data()[i-sqs];                 
+                   
+        for(i = utsize-1; i >= 0; i--){  //Gauss elimination
+                pvt = tmp[i + utsize*i];    //get pivot 
+                for(j = i; j < totsize; j++) tmp[i + utsize*j] /= pvt;  //normalize row                                    
+                for(j = 0;j < i; j++){ //subtract normalized row from above ones
+                        pvt = tmp[j + utsize*i]; //get pivot
+                        for(k = 0; k < totsize; k++)    
+                                tmp[j + utsize*k] -= pvt * tmp[i + utsize*k]; //create zero col above                                             
+                }
+                                                                                  
+        }    
+                    
+        for(i = 0; i < utsize*mbcol; i++)
+                mC._data()[i] = tmp[i+sqs];             
+        
+        return true;
+}
+
 
 //! extended class representing function \f$f(x) = Ax+B\f$
@@ -229 +273 @@
                                 //for(k = 0; k < Models.size(); k++){                                                   
                                 //      if( sum((tmpQrv(j_vec)).findself(Models(k).rv_ret)) > (-1) ){
-                                //      //TODO is tmpQrv(j) in kth Models RV
+                                //      //tTODO is tmpQrv(j) in kth Models RV
                                 //              //if( (Models(k)).rv_ret.findself_ids(trs_crv) ){//???????????????????
                                 //              // is kth Models rv_ret subset of trs_crv
@@ -243 +287 @@
                                 
                                 //if(selectedModel == -1) {cout << "NO MODEL" << endl;return mat("0");}//ERROR - inconsistent model data;                               
-                        //!!TODO!!if(NOT((tmpQrv(j_vec)).findself(model_rv_ret))) {cout << "NO MODEL" << endl;return mat("0");}//ERROR - inconsistent model data;                               
+                        //!!tTODO!!if(NOT((tmpQrv(j_vec)).findself(model_rv_ret))) {cout << "NO MODEL" << endl;return mat("0");}//ERROR - inconsistent model data;                              
                                 //use kth Model to convert tmpQrv memeber to trs_crv memeber
 
@@ -445 +489 @@
                 qr(pre_qr, tmpQ, post_qr);
 //cout << "preQR " << pre_qr << endl << "postQR" << post_qr << endl;
-                mat qrA = get_qr_submatrix(0);          
+                mat qrA = -get_qr_submatrix(0);         
                 mat qrB = get_qr_submatrix(1);
                 mat qrC = get_qr_submatrix(2);
 //cout << "A " << qrA << "\n B " << qrB << "\n C " << qrC << endl;
 
-                L = - inv(qrA)*qrB; ///////// INVERSE OF TRIANGLE MATRIX! better?
+                //mat L1 = - inv(qrA)*qrB; 
+                //mat L2;
+                bool invmult = ldutg(qrA, qrB, L);
+                bdm_assert(invmult, "Matrix inversion error!");
+                
+                /*if(L1 != (-L2)){
+                         cout << "qrA" << qrA << endl;
+                         int l;
+                         for(l = 0; l < qrA.rows()*qrA.cols();l++) cout << qrA._data()[l] << " ";
+                         cout << endl << "qrB" << qrB << endl;
+                         for(l = 0; l < qrB.rows()*qrB.cols();l++) cout << qrB._data()[l] << " ";
+                         cout << endl << L1 << endl << (-L2) << endl;
+                        }
+                
+                bdm_assert(L1 == (-L2), "Matrix compare error!");
+                L = L1;*/
+                                
+                // ldutg is faster matrix inv&mult (like Matlab's \ operator) function than inv
+                // it uses Gauss elimination
+                // BUT based on NOT RECOMENDED direct data access method in mat class
+                // even faster implementation could be implemented using fix double arrays              
+                
                 tolC = qrC;
         }