lq_ctrl.h
00001 #include "ctrlbase.h" 00002 00003 namespace bdm { 00004 00005 const bool STRICT_RV = true; //empty RV NOT allowed 00006 00009 inline bool ldutg(mat &_utA, mat &_mB, mat &_mC){ 00010 int utsize = _utA.rows(); 00011 00012 if(utsize != _utA.cols()) return false; //utA not square 00013 if(utsize != _mB.rows()) return false; //incorrect mB size 00014 00015 int mbcol = _mB.cols(); 00016 int i, j, k; 00017 00018 double pvt; 00019 double *utA = _utA._data(); 00020 double *mB = _mB._data(); 00021 00022 _mC.set_size(utsize, mbcol); 00023 00024 double *mC = _mC._data(); 00025 00026 //copy data 00027 for(i = 0; i < utsize*mbcol; i++) 00028 mC[i] = mB[i]; 00029 00030 for(i = utsize-1; i >= 0; i--){ //Gauss elimination 00031 pvt = utA[i + utsize*i]; //get pivot 00032 for(j = 0; j < mbcol; j++) mC[i + utsize*j] /= pvt; //normalize row - only part on the right 00033 for(j = 0;j < i; j++){ //subtract normalized row from above ones 00034 pvt = utA[j + utsize*i]; //get pivot 00035 for(k = 0; k < mbcol; k++) //goes from utsize - do not need make matrix on the left = I 00036 mC[j + utsize*k] -= pvt * mC[i + utsize*k]; //create zero col above 00037 } 00038 00039 } 00040 00041 return true; 00042 } 00043 00044 00046 class linfnEx: public linfn { 00047 public: 00049 RV rv_ret; 00051 linfnEx ( ) : linfn() { }; 00052 linfnEx ( const mat &A0, const vec &B0 ) : linfn(A0, B0) { }; 00053 }; 00054 00056 class LQG_universal : public Controller{ 00057 public: 00060 Array<quadraticfn> Losses; 00062 quadraticfn finalLoss; 00064 Array<linfnEx> Models; 00065 00068 00070 int horizon; 00071 00073 LQG_universal() { 00074 horizon = 0; 00075 curtime = -1; 00076 } 00077 00078 protected: 00080 mat L; 00081 00083 mat pre_qr; 00084 00086 mat post_qr; 00087 00089 mat tolC; 00090 00091 int curtime; 00092 00093 public: 00095 virtual void redesign() { 00096 if (curtime == -1) curtime = horizon; 00097 00098 if (curtime > 0){ 00099 curtime--; 00100 generateLmat(curtime); 00101 } 00102 } 00104 virtual vec ctrlaction ( const vec &cond ) const { 00105 return L * concat(cond, 1.0); 00106 } 00107 00108 void from_setting ( const Setting &set ) { 00109 UI::get(Losses, set, "losses",UI::compulsory); 00110 UI::get(Models, set, "models",UI::compulsory); 00111 } 00113 const RV& _rv() { 00114 return rv; 00115 } 00117 const RV& _rvc() { 00118 return rvc; 00119 } 00120 00121 void set_rvc(RV _rvc) {rvc = _rvc;} 00122 00124 virtual void log_register ( logger &L, const string &prefix ) { } 00126 virtual void log_write ( ) const { } 00127 00129 mat getL(){ return L; } 00130 00131 void resetTime() { curtime = -1; } 00132 00134 virtual void validate(){ 00135 /* 00136 RV:findself hleda cela rv jako vektory, pri nenalezeni je -1 00137 RV:dataind hleda datove slozky, tedy indexy v poli skalaru, pri nenalezeni vynecha 00138 */ 00139 // (0) nonempty 00140 bdm_assert((Models.size() > 0), "VALIDATION FAILED! Models array empty."); 00141 bdm_assert((Losses.size() > 0), "VALIDATION FAILED! Losses array empty."); 00142 if( (Models.size() <= 0) || (Losses.size() <= 0) ) return; 00143 00144 // (1) test Models array rv - acceptable rv is only part/composition of LQG_universal::rv, LQG_universal::rvc and const 1 00145 RV accept_total; 00146 accept_total = rv; 00147 accept_total.add(rvc); 00148 accept_total.add(RV("1", 1, 0)); 00149 00150 int i, j; 00151 ivec finding1; 00152 00153 for(i = 0; i < Models.length(); i++){ 00154 finding1 = Models(i).rv.findself(accept_total); 00155 00156 bdm_assert( !(STRICT_RV && (finding1.size() <= 0)), "VALIDATION FAILED! Empty RV used."); 00157 00158 for(j = 0; j < finding1.size(); j++){ 00159 bdm_assert( ( finding1(j) > (-1) ), "VALIDATION FAILED! Provided input RV for some Models function is unknown, forbidden or recursive."); 00160 if(finding1(j) <= (-1) ) return; //rv element is not part of admissible rvs => error 00161 } 00162 } 00163 00164 // (3) test Losses array - acceptable rv is only part/composition of LQG_universal::rv, LQG_universal::rvc, Models rv_ret and const 1 00165 for(i = 0; i < Models.length(); i++) accept_total.add(Models(i).rv_ret); //old accept_total from (1) + all rv_ret from Models 00166 00167 for(i = 0; i < Losses.length(); i++){ 00168 finding1 = Losses(i).rv.findself(accept_total); 00169 00170 bdm_assert( !(STRICT_RV && (finding1.size() <= 0)), "VALIDATION FAILED! Empty RV used."); 00171 00172 for(j = 0; j < finding1.size(); j++){ 00173 bdm_assert( ( finding1(j) > (-1) ), "VALIDATION FAILED! Unacceptable RV used in some Losses function."); 00174 if(finding1(j) <= (-1) ) return; //rv element is not part of admissible rvs => error 00175 } 00176 } 00177 00178 // same for finalLoss 00179 finding1 = finalLoss.rv.findself(accept_total); 00180 00181 bdm_assert( !(STRICT_RV && (finding1.size() <= 0)), "VALIDATION FAILED! Empty RV used."); 00182 00183 for(j = 0; j < finding1.size(); j++){ 00184 bdm_assert( ( finding1(j) > (-1) ), "VALIDATION FAILED! Unacceptable RV used in finalLoss function."); 00185 if(finding1(j) <= (-1) ) return; //rv element is not part of admissible rvs => error 00186 } 00187 } 00188 00189 private: 00190 RV trs_crv; 00191 00192 mat getMatRow (quadraticfn sourceQfn){ //returns row of matrixes crated from quadratic function 00193 00194 mat tmpMatRow; //tmp variable for row of matrixes to be returned 00195 tmpMatRow.set_size(sourceQfn.Q.rows(), trs_crv.countsize()); //(rows, cols) 00196 tmpMatRow.zeros(); 00197 00198 //set data in tmpMatRow - other times then current replace using model 00199 RV tmpQrv = sourceQfn.rv; 00200 00201 ivec j_vec(1); 00202 vec copycol; 00203 ivec copysource; 00204 for(int j = 0; j < tmpQrv.length(); j++){ 00205 j_vec(0) = j; 00206 00207 if( (sum(tmpQrv(j_vec).findself(trs_crv)) > (-1)) ) {//sum is only formal, summed vector is in fact scalar 00208 //jth element of tmpQrv is also element of trs_crv with a proper time 00209 00210 ivec copytarget = (tmpQrv(j_vec)).dataind(trs_crv); //get target column position in tmpMatRow 00211 ivec copysource = (tmpQrv(j_vec)).dataind(tmpQrv); //get source column position in Losses(i).Q 00212 if(copytarget.size() != copysource.size()) {return mat(0); /*error*/} 00213 for(int k = 0; k < copysource.size(); k++){ 00214 copycol = sourceQfn.Q._Ch().get_col(copysource(k)); 00215 copycol += tmpMatRow.get_col(copytarget(k)); 00216 tmpMatRow.set_col(copytarget(k), copycol); 00217 } 00218 } 00219 else { 00221 00222 //use kth Model to convert tmpQrv memeber to trs_crv memeber 00223 00224 //get submatrix from Q which represents jth tmpQrv data 00225 copysource = (tmpQrv(j_vec)).dataind(tmpQrv); //get source column position in Losses(i).Q 00226 mat copysubmat; 00227 copysubmat.set_size(sourceQfn.Q.rows(), copysource.size()); //(rows, cols) 00228 copysubmat.zeros(); 00229 vec copycol; 00230 00231 int k; 00232 for(k = 0; k < copysource.size(); k++){ 00233 copycol = sourceQfn.Q._Ch().get_col(copysource(k)); 00234 copysubmat.set_col(k, copycol); 00235 } 00236 00237 //check every Models element if it is a proper substitution: tmpQrv(j_vec) memeber of rv_ret 00238 for(k = 0; k < Models.size(); k++){ 00239 if( sum((tmpQrv(j_vec)).findself(Models(k).rv_ret)) > (-1) ){ //formal sum, find usable model 00240 //check if model is correct 00241 ivec check = (Models(k).rv).findself(trs_crv); 00242 if(sum(check) <= -check.size()){ 00243 bdm_assert (false , "Incorrect Model: Unusable Models element!" ); 00244 continue; 00245 } 00246 00247 //create transformed submatrix 00248 mat transsubmat = copysubmat * ((Models(k)).A); 00249 00250 //put them on a right place in tmpQrv 00251 ivec copytarget = (Models(k)).rv.dataind(trs_crv); //get target column position in tmpMatRow 00252 00253 //copy transsubmat into tmpMatRow with adding to current one 00254 int l; 00255 for(l = 0; l < copytarget.size(); l++){ 00256 copycol = tmpMatRow.get_col(copytarget(l)); 00257 copycol += transsubmat.get_col(l); 00258 tmpMatRow.set_col(copytarget(l), copycol); 00259 } 00260 00261 //if linear fnc constant element vec B is nonzero 00262 vec constElB = (Models(k)).B; 00263 if(prod(constElB) != 0){ 00264 //copy transformed constant vec into last (1's) col in tmpMatRow 00265 int lastcol = tmpMatRow.cols() - 1; 00266 copycol = tmpMatRow.get_col(lastcol); 00267 copycol += (copysubmat * ((Models(k)).B)); 00268 tmpMatRow.set_col(lastcol, copycol); 00269 } 00270 } 00271 00272 } 00273 00274 00275 } 00276 } 00277 00278 return tmpMatRow; 00279 } 00280 00282 void build_pre_qr(bool next) { 00283 int i; 00284 //used fake quadratic function from tolC matrix 00285 quadraticfn fakeQfn; 00286 00287 //set proper size of pre_qr matrix 00288 int rows = 0; 00289 for(i = 0; i < Losses.size(); i++) 00290 rows += Losses(i).Q.rows(); 00291 if(!next) rows += finalLoss.Q.rows(); 00292 else{ 00293 //used fake quadratic function from tolC matrix 00294 //setup fakeQfn 00295 fakeQfn.Q.setCh(tolC); 00296 RV fakeM1; 00297 fakeM1 = rvc; 00298 fakeM1.add(RV("1", 1, 0)); 00299 fakeM1.t_plus(1); //RV in time t+1 => necessary use of Model to get RV in time t 00300 fakeM1.set_time((RV("1", 1, 1).findself(fakeM1))(0) , 0); 00301 00302 fakeQfn.rv = fakeM1; 00303 00304 rows += fakeQfn.Q.rows(); 00305 } 00306 00307 pre_qr.set_size(rows, trs_crv.countsize()); //(rows, cols) 00308 pre_qr.zeros(); 00309 00310 //fill pre_qr matrix for each Losses quadraticfn 00311 int rowIndex = 0; 00312 mat tmpMatRow; 00313 for(i = 0; i < Losses.size(); i++) { 00314 rows = Losses(i).Q.rows(); 00315 00316 //compute row matrix and insert it on proper place in pre_qr 00317 tmpMatRow = getMatRow(Losses(i)); 00318 00319 //copy tmpMatRow in pre_qr 00320 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) 00321 rowIndex += rows; 00322 } 00323 00324 if(!next) { 00325 tmpMatRow = getMatRow(finalLoss); 00326 pre_qr.set_submatrix(rowIndex, (rowIndex + finalLoss.Q.rows() - 1), 0, (trs_crv.countsize() - 1), tmpMatRow); //(int r1, int r2, int c1, int c2, const Mat< Num_T > &m) 00327 } 00328 else { //next 00329 //based on tolC but time must be shifted by one - all implemented in getMatRow method 00330 00331 //get matrix row via getMatRow method 00332 tmpMatRow = getMatRow(fakeQfn); 00333 00334 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) 00335 } 00336 } 00337 00338 mat get_qr_submatrix(int submatidx) { 00339 /* 00340 |rv||rvc||1| 00341 00342 AAAABBBBBBBB 00343 AAABBBBBBBB 00344 AABBBBBBBB 00345 ABBBBBBBB 00346 CCCCCCCC 00347 CCCCCCC 00348 CCCCCC 00349 CCCCC 00350 CCCC 00351 CCC 00352 CC 00353 C 00354 */ 00362 int sizeA = rv.countsize(); 00363 int colsB = post_qr.cols() - sizeA; 00364 // rowsB = sizeA; 00365 // colsC = colsB; 00366 //not required whole C - it is triangular 00367 //=> NOT int rowsC = post_qr.rows() - sizeA; 00368 //=> int sizeC = colsB; 00369 00370 mat qr_submat; 00371 00372 if(submatidx == 0) qr_submat = post_qr.get(0, (sizeA - 1), 0, (sizeA - 1)); //(int r1, int r2, int c1, int c2) 00373 else if(submatidx == 1) qr_submat = post_qr.get(0, (sizeA - 1), sizeA, (post_qr.cols() - 1)); 00374 else { 00375 if(post_qr.cols() > post_qr.rows()) { //extend post_qr matrix to be at least square 00376 post_qr.set_size(post_qr.cols(), post_qr.cols(), true); 00377 } 00378 00379 qr_submat = post_qr.get(sizeA, (sizeA + colsB - 1), sizeA, (post_qr.cols() - 1)); 00380 00381 } 00382 00383 return qr_submat; 00384 } 00385 00386 void generateLmat(int timestep){ 00391 00392 trs_crv = rv; //transformed crv only in proper times, form [rv, rvc, 1] 00393 trs_crv.add(rvc); 00394 trs_crv.add(RV("1", 1, 0)); 00395 00397 if(timestep == (horizon-1)) 00398 build_pre_qr(0); 00399 00401 else 00402 build_pre_qr(1); 00403 00404 mat tmpQ; 00405 qr(pre_qr, tmpQ, post_qr); 00406 00407 mat qrA = -get_qr_submatrix(0); 00408 mat qrB = get_qr_submatrix(1); 00409 mat qrC = get_qr_submatrix(2); 00410 00411 //mat L = inv(qrA)*qrB; 00412 bool invmult = ldutg(qrA, qrB, L); 00413 bdm_assert(invmult, "Matrix inversion error!"); 00414 00415 // ldutg is faster matrix inv&mult (like Matlab's \ operator) function than inv 00416 // it uses Gauss elimination 00417 // BUT based on NOT RECOMENDED direct data access method in mat class 00418 // even faster implementation could be implemented using fix double arrays 00419 00420 tolC = qrC; 00421 } 00422 00423 }; 00424 00425 } // namespace
Generated on 2 Dec 2013 for mixpp by
1.4.7
