root/library/bdm/design/lq_ctrl.h @ 1193

Revision 1193, 15.3 kB (checked in by vahalam, 14 years ago)

oprava chyb nalezenych pomoci testu

Line 
1#include "ctrlbase.h"
2
3namespace bdm {
4
5const bool STRICT_RV = true; //empty RV NOT allowed
6
7//! extended class representing function \f$f(x) = Ax+B\f$
8class linfnEx: public linfn {
9  public:
10    //! Identification of returned value \f$f(x)\f$
11    RV rv_ret;
12        //!default constructor
13    linfnEx ( ) : linfn() { };
14    linfnEx ( const mat &A0, const vec &B0 ) : linfn(A0, B0) { };
15};
16
17  //! Universal LQG controller
18class LQG_universal : public Controller{
19public:
20        //! Controller inputs
21                //! loss function
22                Array<quadraticfn> Losses;
23                //! loss in final time
24                quadraticfn finalLoss;
25                //! model of evolutin
26                Array<linfnEx> Models;
27//              RV model_rv_ret;
28
29                //! control law rv is public member in Controller class 
30                //! input data rvc is protected member in Controller class
31                 
32                //! control horizon
33                int horizon;
34
35        //! Constructor
36                LQG_universal() {
37                        horizon = 0;
38                        curtime = -1;
39                }
40               
41  protected:
42    //! control law: rv = L [rvc, 1]
43    mat L;
44   
45    //! Matrix pre_qr
46    mat pre_qr;
47   
48    //! Matrix post_qr
49    mat post_qr;
50
51        //! time+1 optimized loss to be added to current one
52        mat tolC;
53
54        int curtime;
55   
56public:
57    //! function redesigning the control strategy
58    virtual void redesign() {
59                if (curtime == -1) curtime = horizon;
60               
61                if (curtime > 0) curtime--;
62
63                if (curtime >= 0){
64                        generateLmat(curtime);
65                }
66
67                //report time 0 reached - LQG designing complete
68                //if(curtime == 0) cout << "time 0 reached" << endl;
69        }
70    //! returns designed control action
71    virtual vec ctrlaction ( const vec &cond ) const {
72        return L * concat(cond, 1.0);
73    }
74
75    void from_setting ( const Setting &set ) {
76      UI::get(Losses, set, "losses",UI::compulsory);
77      UI::get(Models, set, "models",UI::compulsory);
78    }
79    //! access function
80    const RV& _rv() {
81        return rv;
82    }
83    //! access function
84    const RV& _rvc() {
85        return rvc;
86    }
87
88        void set_rvc(RV _rvc) {rvc = _rvc;}
89
90    //! register this controller with given datasource under name "name"
91    virtual void log_register ( logger &L, const string &prefix ) { }
92    //! write requested values into the logger
93    virtual void log_write ( ) const { }
94
95        //! access debug function
96        mat getL(){ return L; }
97
98        void resetTime() { curtime = -1; }
99
100        //! check if model and losses is correct and consistent
101        virtual void validate(){
102                /*
103                        RV:findself hleda cela rv jako vektory, pri nenalezeni je -1
104                        RV:dataind hleda datove slozky, tedy indexy v poli skalaru, pri nenalezeni vynecha
105                */
106                // (0) nonempty
107                bdm_assert((Models.size() > 0), "VALIDATION FAILED! Models array empty.");
108                bdm_assert((Losses.size() > 0), "VALIDATION FAILED! Losses array empty.");
109                if( (Models.size() <= 0) || (Losses.size() <= 0) ) return;
110
111                // (1) test Models array rv - acceptable rv is only part/composition of LQG_universal::rv, LQG_universal::rvc and const 1
112                RV accept_total;
113                accept_total = rv;
114                accept_total.add(rvc);
115                accept_total.add(RV("1", 1, 0));
116               
117                int i, j;
118                ivec finding1;
119
120                for(i = 0; i < Models.length(); i++){
121                        finding1 = Models(i).rv.findself(accept_total); 
122
123                        bdm_assert( !(STRICT_RV && (finding1.size() <= 0)), "VALIDATION FAILED! Empty RV used.");
124
125                        for(j = 0; j < finding1.size(); j++){                           
126                                bdm_assert( ( finding1(j) > (-1) ), "VALIDATION FAILED! Provided input RV for some Models function is unknown, forbidden or recursive.");                                                       
127                                if(finding1(j) <= (-1) ) return; //rv element is not part of admissible rvs => error
128                        }                       
129                }               
130               
131                //NOT!!! (2) test Models array rv_ret - each array element's rv_ret must be unique (except const 1)
132                //RV unique_rv_ret;
133                //unique_rv_ret = Models(0).rv_ret;
134
135                //for(i = 1; i < Models.length(); i++){
136                //      finding1 = Models(i).rv_ret.findself(unique_rv_ret);
137
138                //      for(j = 0; j < finding1.size(); j++){                                                           
139                //              if(Models(i).rv_ret.name(j) == "1") continue; // except const 1
140
141                //              bdm_assert((finding1(j) == (-1) ), "VALIDATION FAILED! Models functions result RV (rv_ret) must be unique.");
142                //              if(finding1(j) != (-1) ) return; //rv_ret element not unique
143                //      }
144
145                //      unique_rv_ret.add(Models(i).rv_ret);
146                //}             
147               
148                // (3) test Losses array - acceptable rv is only part/composition of LQG_universal::rv, LQG_universal::rvc, Models rv_ret and const 1
149                for(i = 0; i < Models.length(); i++) accept_total.add(Models(i).rv_ret); //old accept_total from (1) + all rv_ret from Models
150               
151                for(i = 0; i < Losses.length(); i++){
152                        finding1 = Losses(i).rv.findself(accept_total);
153
154                        bdm_assert( !(STRICT_RV && (finding1.size() <= 0)), "VALIDATION FAILED! Empty RV used.");
155
156                        for(j = 0; j < finding1.size(); j++){
157                                bdm_assert( ( finding1(j) > (-1) ), "VALIDATION FAILED! Unacceptable RV used in some Losses function.");
158                                if(finding1(j) <= (-1) ) return; //rv element is not part of admissible rvs => error
159                        }
160                }       
161
162                // same for finalLoss
163                finding1 = finalLoss.rv.findself(accept_total);
164
165                bdm_assert( !(STRICT_RV && (finding1.size() <= 0)), "VALIDATION FAILED! Empty RV used.");
166
167                for(j = 0; j < finding1.size(); j++){
168                        bdm_assert( ( finding1(j) > (-1) ), "VALIDATION FAILED! Unacceptable RV used in finalLoss function.");
169                        if(finding1(j) <= (-1) ) return; //rv element is not part of admissible rvs => error
170                }
171        }
172
173private:
174        RV trs_crv;
175
176        //! compute complete RV from all of RVs used in Losses array
177        /*RV getCompleteRV() {
178                RV cRv; //complete RV
179
180                //cRv has form [rv, "other", 1]
181                cRv = rv; //add rv
182
183                //add "other"
184                for(int i = 0; i < Losses.size(); i++)
185                        cRv.add(Losses(i).rv);
186
187                cRv.add(rvOne); //add 1
188
189                return cRv;
190        }*/
191
192        mat getMatRow (quadraticfn sourceQfn){ //returns row of matrixes crated from quadratic function
193               
194                mat tmpMatRow; //tmp variable for row of matrixes to be returned       
195                tmpMatRow.set_size(sourceQfn.Q.rows(), trs_crv.countsize()); //(rows, cols)
196                tmpMatRow.zeros();
197
198                //set data in tmpMatRow - other times then current replace using model
199                RV tmpQrv = sourceQfn.rv;
200                ivec j_vec(1);
201                for(int j = 0; j < tmpQrv.length(); j++){                       
202                        j_vec(0) = j;
203                       
204                        if( (tmpQrv.time(j) == 0) && (sum(tmpQrv(j_vec).findself(trs_crv)) > (-1)) ) {//sum is only formal, summed vector is in fact scalar
205                                //jth element of tmpQrv is also element of trs_crv with a proper time
206                                ivec copytarget = (tmpQrv(j_vec)).dataind(trs_crv); //get target column position in tmpMatRow
207                                ivec copysource = (tmpQrv(j_vec)).dataind(tmpQrv); //get source column position in Losses(i).Q
208                                if(copytarget.size() != copysource.size()) {return mat(0); /*error*/}
209                                vec copycol;
210                                for(int k = 0; k < copysource.size(); k++){
211                                        copycol = sourceQfn.Q._Ch().get_col(copysource(k));
212                                        tmpMatRow.set_col(copytarget(k), copycol);
213                                }                                       
214                        }
215                        else {
216//cout << "USING MODEL" << endl;
217                                //jth tmpQrv element is not in trs_crv -> using Model to teplace it
218                                ivec copysource;// = (tmpQrv(j_vec)).findself(tmpQrv); //get source column position in Losses(i).Q
219
220                                //int selectedModel = -1;
221
222                                //int k;
223                                ////find first usable replacement in Model                             
224                                //for(k = 0; k < Models.size(); k++){                                                   
225                                //      if( sum((tmpQrv(j_vec)).findself(Models(k).rv_ret)) > (-1) ){
226                                //      //TODO is tmpQrv(j) in kth Models RV
227                                //              //if( (Models(k)).rv_ret.findself_ids(trs_crv) ){//???????????????????
228                                //              // is kth Models rv_ret subset of trs_crv
229
230                                //              selectedModel = k;
231                                //              break;
232
233                                //              //}
234                                //      }
235                                //}
236                               
237                                //!model is model_rv_ret = sum(Array<linfn>) = sum( A1*rv + B1 + ... + An*rv + Bn)
238                               
239                                //if(selectedModel == -1) {cout << "NO MODEL" << endl;return mat("0");}//ERROR - inconsistent model data;                               
240                        //!!TODO!!if(NOT((tmpQrv(j_vec)).findself(model_rv_ret))) {cout << "NO MODEL" << endl;return mat("0");}//ERROR - inconsistent model data;                               
241                                //use kth Model to convert tmpQrv memeber to trs_crv memeber
242
243                                //get submatrix from Q which represents jth tmpQrv data
244                                copysource = (tmpQrv(j_vec)).dataind(tmpQrv); //get source column position in Losses(i).Q                               
245                                mat copysubmat;
246                                copysubmat.set_size(sourceQfn.Q.rows(), copysource.size()); //(rows, cols)
247                                copysubmat.zeros();
248                                vec copycol;
249                               
250                                int k;
251                                for(k = 0; k < copysource.size(); k++){
252                                        copycol = sourceQfn.Q._Ch().get_col(copysource(k));
253                                        copysubmat.set_col(k, copycol);
254                                }
255
256                                //check every Models element if it is a proper substitution: tmpQrv(j_vec) memeber of rv_ret
257                                for(k = 0; k < Models.size(); k++){
258                                        if( sum((tmpQrv(j_vec)).findself(Models(k).rv_ret)) > (-1) ){ //formal sum, find usable model
259                                                //check if model is correct
260                                                ivec check = (Models(k).rv).findself(trs_crv);
261                                                if(sum(check) <= -check.size()){
262                                                        bdm_assert (false , "Incorrect Model: Unusable Models element!" );                                               
263                                                        continue;
264                                                }
265
266                                                //create transformed submatrix
267                                                mat transsubmat = copysubmat * ((Models(k)).A);
268
269                                                //put them on a right place in tmpQrv
270                                                ivec copytarget = (Models(k)).rv.dataind(trs_crv); //get target column position in tmpMatRow
271                                                                                               
272                                                //copy transsubmat into tmpMatRow with adding to current one
273                                                //      tmpMatRow(new) = tmpMatRow(old) + transsubmat /all in proper indices
274                                                int l;
275                                                for(l = 0; l < copytarget.size(); l++){                                 
276                                                        copycol = tmpMatRow.get_col(copytarget(l));                                     
277                                                        copycol += transsubmat.get_col(l);                                                             
278                                                        tmpMatRow.set_col(copytarget(l), copycol);                                                             
279                                                }
280
281                                                //if linear fnc constant element vec B is nonzero
282                                                vec constElB = (Models(k)).B;                           
283                                                if(prod(constElB) != 0){
284                                                        //copy transformed constant vec into last (1's) col in tmpMatRow
285                                                        int lastcol = tmpMatRow.cols() - 1;
286                                                        copycol = tmpMatRow.get_col(lastcol);
287                                                        copycol += (copysubmat * ((Models(k)).B));
288                                                        tmpMatRow.set_col(lastcol, copycol);
289                                                }
290                                        }
291
292                                }
293
294                               
295                        }
296                }
297
298                return tmpMatRow;
299        }
300
301        //! create first(0) or other (1) pre_qr matrix
302        void build_pre_qr(bool next) {
303                int i;
304                //used fake quadratic function from tolC matrix
305                quadraticfn fakeQfn;
306
307                //RV pretrs_crv = getCompleteRV(); // crv before transformation based on Losses array
308
309                //set proper size of pre_qr matrix
310                int rows = 0;
311                for(i = 0; i < Losses.size(); i++)
312                        rows += Losses(i).Q.rows();
313                if(!next) rows += finalLoss.Q.rows();
314                else{
315                        //used fake quadratic function from tolC matrix
316                        //setup fakeQfn
317                        fakeQfn.Q.setCh(tolC);
318                        RV fakeM1;
319                        fakeM1 = rvc;
320                        fakeM1.add(RV("1", 1, 0));
321                        fakeM1.t_plus(1); //RV in time t+1 => necessary use of Model to get RV in time t
322                        fakeQfn.rv = fakeM1;
323                       
324                        rows += fakeQfn.Q.rows();
325                }
326//cout << "buildpreqr trscrv: " << trs_crv << " of size " << trs_crv.countsize() << endl;
327                pre_qr.set_size(rows, trs_crv.countsize()); //(rows, cols)
328                pre_qr.zeros();
329
330                //fill pre_qr matrix for each Losses quadraticfn               
331                int rowIndex = 0;
332                mat tmpMatRow;
333                for(i = 0; i < Losses.size(); i++) {
334                        rows = Losses(i).Q.rows();
335                       
336                        //compute row matrix and insert it on proper place in pre_qr
337                        tmpMatRow = getMatRow(Losses(i));
338//cout << "tmpMatRow no " << i << endl << tmpMatRow << endl;
339                        //copy tmpMatRow in pre_qr
340
341                /*cout << "submatrix ( " << rowIndex << ", " <<
342                        (rowIndex + rows - 1) << ", 0, " << (trs_crv.countsize() - 1) << ")" << endl;
343                cout << "seting with submatrix of rows " << tmpMatRow.rows() << " and cols " << tmpMatRow.cols() <<
344                        "and data " << endl << tmpMatRow << endl;*/
345
346                        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)
347                        rowIndex += rows;
348                }
349
350                if(!next) {                     
351                        tmpMatRow = getMatRow(finalLoss);                       
352                        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)               
353                }
354                else { //next
355                        //based on tolC but time must be shifted by one - all implemented in getMatRow method
356                               
357                        //get matrix row via getMatRow method
358                        tmpMatRow = getMatRow(fakeQfn);
359                /*cout << "submatrix ( " << rowIndex << ", " <<
360                        (rowIndex + fakeQfn.Q.rows() - 1) << ", 0, " << (trs_crv.countsize() - 1) << ")" << endl;
361                cout << "seting with submatrix of rows " << tmpMatRow.rows() << " and cols " << tmpMatRow.cols() <<
362                        "and data " << endl << tmpMatRow << endl;*/
363                        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)         
364                //cout << "NEXT 3" << endl;
365                }
366//cout << "last tmpMatRow  " << endl << tmpMatRow << endl;
367        }       
368
369        mat get_qr_submatrix(int submatidx) {
370        /*
371                |rv||rvc||1|
372
373                AAAABBBBBBBB
374                 AAABBBBBBBB
375                  AABBBBBBBB
376                   ABBBBBBBB
377                    CCCCCCCC
378                         CCCCCCC
379                          CCCCCC
380                           CCCCC
381                            CCCC
382                                 CCC
383                                  CC
384                                   C
385        */
386        /*!
387                submatidx | get_submatrix
388                ----------|--------------
389                    0     |      A
390                        1     |          B
391                        2+        |              C
392        */
393                int sizeA = rv.countsize();
394                int colsB = post_qr.cols() -  sizeA;
395                //  rowsB = sizeA;
396                //  colsC = colsB;
397                //not required whole C - it is triangular
398                //=> NOT int rowsC = post_qr.rows() - sizeA;
399                //=> int sizeC = colsB;
400
401                mat qr_submat;
402               
403                if(submatidx == 0) qr_submat            = post_qr.get(0,                (sizeA - 1),                    0,              (sizeA - 1));  //(int r1, int r2, int c1, int c2)
404                else if(submatidx == 1) qr_submat       = post_qr.get(0,                (sizeA - 1),                    sizeA,  (post_qr.cols() - 1));
405                else qr_submat                                          = post_qr.get(sizeA,    (sizeA + colsB - 1),    sizeA,  (post_qr.cols() - 1));
406       
407                return qr_submat;
408        }
409
410        void generateLmat(int timestep){
411                //! control strategy matrix L is based on loss in time:
412                //!             time = horizon                  loss = finalLoss
413                //!             time = horizon - 1              loss = sum(Losses)(time) + finalLoss
414                //!             time = horizon - k > 1  loss = sum(Losses)(time) + tolC time+1 loss
415
416                trs_crv = rv; //transformed crv only in proper times, form [rv, rvc, 1]
417                trs_crv.add(rvc);
418                trs_crv.add(RV("1", 1, 0));
419               
420                //!first time, time = horizon - 1
421                if(timestep == (horizon-1))             
422                        build_pre_qr(0);
423               
424                //!other times         
425                else
426                        build_pre_qr(1);
427
428                mat tmpQ;               
429                qr(pre_qr, tmpQ, post_qr);
430//cout << "preQR " << pre_qr << endl << "postQR" << post_qr << endl;
431                mat qrA = get_qr_submatrix(0);
432                mat qrB = get_qr_submatrix(1);
433                mat qrC = get_qr_submatrix(2);
434//cout << "A " << qrA << "\n B " << qrB << "\n C " << qrC << endl;
435
436                L = - inv(qrA)*qrB; ///////// INVERSE OF TRIANGLE MATRIX! better?
437                tolC = qrC;
438        }
439
440};
441
442class LQG_recedinghorizon : public LQG_universal {
443protected:
444        //!total_curtime is curtime for total_horizon
445        int total_curtime;
446public:
447        //! LQG_universal::horizon means shorter receding horizon for designing control strategy
448        //! total_horizon is longer total horizon
449        int total_horizon;
450       
451        //!constructor
452        LQG_recedinghorizon() : LQG_universal() {
453                        total_horizon = 0;
454                        total_curtime = 0;
455                }
456
457        virtual void redesign() {
458                if (total_curtime < total_horizon){
459                        for(int i = 0; i < horizon - 1; i++) LQG_universal::redesign();
460                        total_curtime++;
461                }               
462        }
463
464        virtual vec ctrlaction ( const vec &cond ) const {
465        //return L * concat(cond, 1.0);
466    }
467
468    //! register this controller with given datasource under name "name"
469    virtual void log_register ( logger &L, const string &prefix ) { }
470    //! write requested values into the logger
471    virtual void log_write ( ) const { }       
472};
473
474} // namespace
Note: See TracBrowser for help on using the browser.