Changeset 1221 for library

Timestamp:

10/20/10 18:41:40 (14 years ago)

Author:

vahalam

Message:

 

Files:

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

library/bdm/design/lq_ctrl.h

@@ -7 +7 @@
 //! 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);
-        
+inline 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(); 
         int totsize = utsize + mbcol;
         int sqs = utsize*utsize;
-        int i, j, k;
-        
-        double pvt;
-        
+        int i, j, k;    
+        
+        double pvt;     
         double tmp[utsize*totsize];
+        double *utA = _utA._data();
+        double *mB = _mB._data();
+                        
+        _mC.set_size(utsize, mbcol);
+        
+        double *mC = _mC._data();
         
         //fill tmp array
         for(i = 0; i < sqs; i++) 
-                tmp[i] = utA._data()[i];
+                tmp[i] = utA[i];
         for(i = sqs; i < utsize*totsize; i++)                           
-                tmp[i] = mB._data()[i-sqs];                 
+                tmp[i] = mB[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 = utsize; j < totsize; j++) tmp[i + utsize*j] /= pvt;     //normalize row - only part on the right                                   
                 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++)    
+                        for(k = utsize; k < totsize; k++) //goes from utsize - do not need make matrix on the left = I  
                                 tmp[j + utsize*k] -= pvt * tmp[i + utsize*k]; //create zero col above                                             
                 }
@@ -43 +45 @@
                     
         for(i = 0; i < utsize*mbcol; i++)
-                mC._data()[i] = tmp[i+sqs];             
+                mC[i] = tmp[i+sqs];             
         
         return true;
@@ -69 +71 @@
                 //! model of evolutin
                 Array<linfnEx> Models;
-//              RV model_rv_ret;
 
                 //! control law rv is public member in Controller class  
@@ -106 +107 @@
                         curtime--;
                         generateLmat(curtime);
-                }
-                //cout << "time " << curtime << endl;
-                //report time 0 reached - LQG designing complete
-                //if(curtime == 0) cout << "time 0 reached" << endl;
+                }               
         }
     //! returns designed control action
@@ -170 +168 @@
                                 if(finding1(j) <= (-1) ) return; //rv element is not part of admissible rvs => error
                         }                       
-                }               
-                
-                //NOT!!! (2) test Models array rv_ret - each array element's rv_ret must be unique (except const 1)
-                //RV unique_rv_ret;
-                //unique_rv_ret = Models(0).rv_ret;
-
-                //for(i = 1; i < Models.length(); i++){
-                //      finding1 = Models(i).rv_ret.findself(unique_rv_ret);
-
-                //      for(j = 0; j < finding1.size(); j++){                                                           
-                //              if(Models(i).rv_ret.name(j) == "1") continue; // except const 1
-
-                //              bdm_assert((finding1(j) == (-1) ), "VALIDATION FAILED! Models functions result RV (rv_ret) must be unique.");
-                //              if(finding1(j) != (-1) ) return; //rv_ret element not unique
-                //      }
-
-                //      unique_rv_ret.add(Models(i).rv_ret);
-                //}             
+                }                       
                 
                 // (3) test Losses array - acceptable rv is only part/composition of LQG_universal::rv, LQG_universal::rvc, Models rv_ret and const 1
@@ -215 +196 @@
 
 private:
-        RV trs_crv;
-
-        //! compute complete RV from all of RVs used in Losses array
-        /*RV getCompleteRV() {
-                RV cRv; //complete RV
-
-                //cRv has form [rv, "other", 1]
-                cRv = rv; //add rv
-
-                //add "other"
-                for(int i = 0; i < Losses.size(); i++)
-                        cRv.add(Losses(i).rv);
-
-                cRv.add(rvOne); //add 1
-
-                return cRv;
-        }*/
+        RV trs_crv;     
 
         mat getMatRow (quadraticfn sourceQfn){ //returns row of matrixes crated from quadratic function
@@ -241 +206 @@
                 //set data in tmpMatRow - other times then current replace using model
                 RV tmpQrv = sourceQfn.rv;
-//cout << "Qrv " << tmpQrv << endl << "total crv " << trs_crv << endl;
+
                 ivec j_vec(1);
                 vec copycol;
@@ -247 +212 @@
                 for(int j = 0; j < tmpQrv.length(); j++){                       
                         j_vec(0) = j;
-//cout << "matrow\n" << tmpMatRow << endl;
-
-                        if( /*(tmpQrv.time(j) == 0) &&*/ (sum(tmpQrv(j_vec).findself(trs_crv)) > (-1)) ) {//sum is only formal, summed vector is in fact scalar
+
+                        if( (sum(tmpQrv(j_vec).findself(trs_crv)) > (-1)) ) {//sum is only formal, summed vector is in fact scalar
                                 //jth element of tmpQrv is also element of trs_crv with a proper time
-//cout << "copy" << endl;
+
                                 ivec copytarget = (tmpQrv(j_vec)).dataind(trs_crv); //get target column position in tmpMatRow
                                 ivec copysource = (tmpQrv(j_vec)).dataind(tmpQrv); //get source column position in Losses(i).Q
@@ -261 +225 @@
                                 }                                       
                         }
-                        else {
-//cout << "USING MODEL on " << tmpQrv(j,j) << endl;
-//cout << "model" << endl;
-                                //jth tmpQrv element is not in trs_crv -> using Model to teplace it
-                                // = (tmpQrv(j_vec)).findself(tmpQrv); //get source column position in Losses(i).Q
-
-                                //int selectedModel = -1;
-
-                                //int k;
-                                ////find first usable replacement in Model                              
-                                //for(k = 0; k < Models.size(); k++){                                                   
-                                //      if( sum((tmpQrv(j_vec)).findself(Models(k).rv_ret)) > (-1) ){
-                                //      //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
-
-                                //              selectedModel = k;
-                                //              break;
-
-                                //              //}
-                                //      }
-                                //}
+                        else {                          
+                                //!model is model_rv_ret = sum(Array<linfn>) = sum( A1*rv + B1 + ... + An*rv + Bn)                              
                                 
-                                //!model is model_rv_ret = sum(Array<linfn>) = sum( A1*rv + B1 + ... + An*rv + Bn)
-                                
-                                //if(selectedModel == -1) {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
 
@@ -319 +259 @@
                                                 ivec copytarget = (Models(k)).rv.dataind(trs_crv); //get target column position in tmpMatRow
                                                                                                 
-                                                //copy transsubmat into tmpMatRow with adding to current one
-                                                //      tmpMatRow(new) = tmpMatRow(old) + transsubmat /all in proper indices
+                                                //copy transsubmat into tmpMatRow with adding to current one                                            
                                                 int l;
                                                 for(l = 0; l < copytarget.size(); l++){                                 
@@ -352 +291 @@
                 int i;
                 //used fake quadratic function from tolC matrix
-                quadraticfn fakeQfn;
-
-                //RV pretrs_crv = getCompleteRV(); // crv before transformation based on Losses array
+                quadraticfn fakeQfn;            
 
                 //set proper size of pre_qr matrix
@@ -370 +307 @@
                         fakeM1.t_plus(1); //RV in time t+1 => necessary use of Model to get RV in time t
                         fakeM1.set_time((RV("1", 1, 1).findself(fakeM1))(0) , 0);
-//cout << fakeM1 << endl;
+
                         fakeQfn.rv = fakeM1;
                         
                         rows += fakeQfn.Q.rows();
                 }
-//cout << "buildpreqr trscrv: " << trs_crv << " of size " << trs_crv.countsize() << endl;
+
                 pre_qr.set_size(rows, trs_crv.countsize()); //(rows, cols)
                 pre_qr.zeros();
@@ -387 +324 @@
                         //compute row matrix and insert it on proper place in pre_qr
                         tmpMatRow = getMatRow(Losses(i));
-//cout << "tmpMatRow no " << i << endl << tmpMatRow << endl;
+
                         //copy tmpMatRow in pre_qr
-
-                /*cout << "submatrix ( " << rowIndex << ", " << 
-                        (rowIndex + rows - 1) << ", 0, " << (trs_crv.countsize() - 1) << ")" << endl;
-                cout << "seting with submatrix of rows " << tmpMatRow.rows() << " and cols " << tmpMatRow.cols() <<
-                        "and data " << endl << tmpMatRow << endl;*/
-
                         pre_qr.set_submatrix(rowIndex, (rowIndex + rows - 1), 0, (trs_crv.countsize() - 1), tmpMatRow);  //(int r1, int r2, int c1, int c2, const Mat<  Num_T > &m)
                         rowIndex += rows;
@@ -406 +337 @@
                         //based on tolC but time must be shifted by one - all implemented in getMatRow method
                                 
-                        //get matrix row via getMatRow method
-                        //cout << "XXXXXXXXXXXXXXX" << endl;
+                        //get matrix row via getMatRow method                   
                         tmpMatRow = getMatRow(fakeQfn);
-                        //cout << tmpMatRow << endl;
-                /*cout << "submatrix ( " << rowIndex << ", " << 
-                        (rowIndex + fakeQfn.Q.rows() - 1) << ", 0, " << (trs_crv.countsize() - 1) << ")" << endl;
-                cout << "seting with submatrix of rows " << tmpMatRow.rows() << " and cols " << tmpMatRow.cols() <<
-                        "and data " << endl << tmpMatRow << endl;*/
+                        
                         pre_qr.set_submatrix(rowIndex, (rowIndex + fakeQfn.Q.rows() - 1), 0, (trs_crv.countsize() - 1), tmpMatRow);  //(int r1, int r2, int c1, int c2, const Mat<  Num_T > &m)         
-                //cout << "NEXT 3" << endl;
-                }
-//cout << "last tmpMatRow  " << endl << tmpMatRow << endl;
+                }
         }       
 
@@ -488 +412 @@
                 mat tmpQ;               
                 qr(pre_qr, tmpQ, post_qr);
-//cout << "preQR " << pre_qr << endl << "postQR" << post_qr << endl;
+
                 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;
-
-                //mat L1 = - inv(qrA)*qrB; 
-                //mat L2;
+
+                //mat L = inv(qrA)*qrB;                 
                 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
@@ -521 +431 @@
 };
 
-class LQG_recedinghorizon : public LQG_universal {
-protected:
-        //!total_curtime is curtime for total_horizon
-        int total_curtime;
-public:
-        //! LQG_universal::horizon means shorter receding horizon for designing control strategy
-        //! total_horizon is longer total horizon
-        int total_horizon;
-        
-        //!constructor
-        LQG_recedinghorizon() : LQG_universal() {
-                        total_horizon = 0;
-                        total_curtime = 0;
-                }
-
-        virtual void redesign() {
-                if (total_curtime < total_horizon){
-                        for(int i = 0; i < horizon - 1; i++) LQG_universal::redesign();
-                        total_curtime++;
-                }               
-        }
-
-        virtual vec ctrlaction ( const vec &cond ) const {
-        //return L * concat(cond, 1.0);
-        return empty_vec;
-    }
-
-    //! register this controller with given datasource under name "name"
-    virtual void log_register ( logger &L, const string &prefix ) { }
-    //! write requested values into the logger
-    virtual void log_write ( ) const { }        
-};
-
 } // namespace