root/applications/pmsm/pmsm_ctrl.h @ 1400

Revision 1398, 46.5 kB (checked in by vahalam, 13 years ago)
Line 
1#ifndef PMSM_CTR_H
2#define PMSM_CTR_H
3
4#include <estim/particles.h>
5#include <design/ctrlbase.h>
6#include <design/lq_ctrl.h>
7#include "pmsm.h"
8#include <ctime>
9
10#include <fstream>
11
12/*! \defgroup PMSM
13@{
14*/
15
16using namespace bdm;
17
18/**/
19class myEKF{
20private:
21        double ial, ibe, ome, the, ual, ube;
22        mat A, C, K, P, Q, R;
23        double a,b,c,d,e,dt;
24public:
25        myEKF():A(4,4),C(2,4),K(4,2),P(4,4),Q(4,4),R(2,2){
26                ial = ibe = ome = the = ual = ube = 0.0;
27               
28                a = 0.9898;
29                b = 0.0072;
30                c = 0.0361;
31                d = 1.0;
32                e = 0.0149;
33                dt = 0.000125;
34               
35                A.zeros();
36                C.zeros();
37                K.zeros();
38                P.zeros();
39                Q.zeros();
40                R.zeros();
41                A(0,0) = a;
42                A(1,1) = a;
43                A(2,2) = d;
44                A(3,3) = 1.0;
45                A(3,2) = dt;
46                C(0,0) = 1.0;
47                C(1,1) = 1.0;
48                Q(0,0) = 0.1;
49                Q(1,1) = 0.1;
50                Q(2,2) = 0.1;
51                Q(3,3) = 0.01;
52                R(0,0) = 0.05;
53                R(1,1) = 0.05;
54        }
55       
56        vec getEst(double yal, double ybe){
57                //yal = yal;
58                //ybe = sqrt(3.0)/3.0 * (2.0*ybe + yal);               
59                //yal = 2.0*yal/3.0;
60                //ybe = 2.0*ybe/3.0;
61       
62                A(0,2) = b*sin(the);
63                A(0,3) = b*ome*cos(the);
64                A(1,2) = -b*cos(the);
65                A(1,3) = b*ome*sin(the);
66                A(2,0) = -e*sin(the);
67                A(2,1) = e*cos(the);
68                A(2,3) = -e*(ial*cos(the)+ibe*sin(the));               
69                       
70                double tial = a*ial + b*ome*sin(the) + c*ual;
71                double tibe = a*ibe - b*ome*cos(the) + c*ube;
72                double tome = d*ome + e*(ibe*cos(the) - ial*sin(the));
73                double tthe = the + dt*ome;             
74       
75                P = A*P*A.transpose() + Q;
76                K = P*C.transpose()*inv(C*P*C.transpose() + R);
77                P = P - K*C*P;       
78       
79                vec yt(2);
80                yt(0) = yal - tial;
81                yt(1) = ybe - tibe; 
82               
83                mat Kyt = K*yt;       
84                tial += Kyt(0,0);
85                tibe += Kyt(1,0);
86                tome += Kyt(2,0);
87                tthe += Kyt(3,0);
88               
89                ial = tial;
90                ibe = tibe;
91                ome = tome;
92                the = tthe;
93
94                vec est(4);
95                est(0) = tial;
96                est(1) = tibe;
97                est(2) = tome;
98                est(3) = tthe;
99               
100                return est;
101        }
102       
103        void setUab(double _ual, double _ube){
104                ual = _ual;
105                ube = _ube;
106        }
107
108};
109/**/
110
111/*! PI Controller*/
112class PI_ctrl: public Controller{
113public:
114        //!integral state
115        double S;
116        double Pd;
117        double Pi;
118        double minOut;
119        double maxOut;
120       
121        PI_ctrl(double Pd0, double Pi0, double minOut0, double maxOut0): S(0.0), Pd(Pd0), Pi(Pi0),minOut(minOut0),maxOut(maxOut0){}
122        virtual vec ctrlaction ( const vec &cond ) {
123                        double tmp;
124                        tmp = Pd*cond(0)+S;
125                       
126                        S += Pi*cond(0);
127                       
128                        if (tmp>maxOut) tmp=maxOut;
129                        if (tmp<minOut) tmp=minOut;
130                        return vec_1(tmp);
131        }
132        void set_params(double Pd0, double Pi0){
133                Pd=Pd0;
134                Pi=Pi0;
135        }
136        void get_params(double &Pd0, double &Pi0){
137                Pd0=Pd;
138                Pi0=Pi;
139        }
140};
141
142/*! \brief Root class for controllers of PMSM
143 *
144 * It defines the interface for controllers that connect to the pmsmDSctrl.
145 * The adapt() function calls bayes() of an internal BM. Note that it is assumed that the estimator is compatible with PMSM system.
146 * I.e. it must have rv={ia, ib} and rvc={ua,ub}, any other BM will not work.
147 *
148 * Two cases are considered:
149 *  - 4 dimensional rv={ia, ib, om, th} and rvc={ua,ub}
150 *  - 2 dimensional rv={om, th} and rvc={ua,ub,ia_{t-1}, ib_{t-1} }
151 *  These are hardcoded and sitched based on dimensionality.
152 */
153class PMSMCtrl: public Controller{
154protected:
155        //! Est where rv and rcv are not used!
156        shared_ptr<BM> Est;
157        //! switch to use or not to use the internal estimator
158        bool estim;
159       
160        // internal quantities filled by PMSMCtrl::ctrlaction()
161        double isa;
162        double isb;
163        double ome;
164        double the;
165        double Ww;
166       
167public:
168
169        PMSMCtrl():Controller() {
170                rv = RV("{ua ub }");
171                rvc = RV("{o_ia o_ib o_ua o_ub o_om o_th Ww }");
172        }
173       
174    void adapt(const itpp::vec& data){
175                if (estim){
176                        vec y=data.get(0,1);
177                        vec u=data.get(2,3);
178                        if (Est->dimension()==2){
179                                static vec cond(4); //
180                                cond.set_subvector(0,1,u);
181                                Est->bayes(y,cond);
182                                cond.set_subvector(2,3,y); // save 1-step delayed vectors
183                               
184                        } else {
185                                Est->bayes(y,u);
186                               
187                        }
188                       
189                }
190        }
191       
192       
193    virtual vec ctrlaction(const itpp::vec& cond) {
194               
195                if (estim){
196                        if (Est->dimension()==2){
197                                vec x_est=Est->posterior().mean();
198                                isa=cond(0);
199                                isb=cond(1);
200                                ome=x_est(0);
201                                the=x_est(1);
202                        } else {
203                        vec x_est=Est->posterior().mean();
204                        isa=x_est(0);
205                        isb=x_est(1);
206                        ome=x_est(2);
207                        the=x_est(3);
208                        }
209                } else {
210                        isa=cond(0);//x_est(0);
211                        isb=cond(1);//x_est(1);
212                        ome=cond(4);//x[2];//x_est(2);
213                        the=cond(5);//x_est(3);
214                }
215                Ww=cond(6);
216               
217                return empty_vec; // dummy return
218        };
219    void from_setting(const libconfig::Setting& set){
220                Controller::from_setting(set);
221                Est=UI::build<BM>(set,"estim",UI::optional);
222                estim = Est; 
223        }
224        void log_register ( logger &L, const string &prefix ) {
225                Controller::log_register(L,prefix);
226                if (estim) Est->log_register(L,prefix);
227        }
228        void log_write() const{
229                if (estim) Est->log_write();
230        }
231};
232//UIREGISTER(PMSMCtrl); -- abstract
233
234class PMSM_PICtrl: public PMSMCtrl{
235public:
236        PI_ctrl Cwq;
237        PI_ctrl Cuq;
238        PI_ctrl Cud;
239/**/ //myEKF mykalm;
240               
241        PMSM_PICtrl():PMSMCtrl(),
242        Cwq(3.0, 3.0*0.000125/0.1, -1200, 1200), 
243        Cuq(20.0, 20.0*0.000125/0.005, -1200, 1200),
244        Cud(20.0, 20.0*0.000125/0.005, -1200, 1200) {}
245               
246       
247        virtual vec ctrlaction(const itpp::vec& cond) {
248                PMSMCtrl::ctrlaction(cond); // fills isa,isb,om,the
249                //cout << isa << " " << isb << " ";
250/**/            //vec est = mykalm.getEst(isa, isb);
251/**/            //isa = est(0);
252/**/            //isb = est(1);
253/**/            //ome = est(2);
254/**/            //the = est(3);
255                //cout << isa << " " << isb << endl;   
256               
257                double Isd = isa*cos(the)+isb*sin(the);
258                double Isq = isb*cos(the)-isa*sin(the);
259               
260                double Iqw=(Cwq.ctrlaction(vec_1(Ww-ome)))(0); // conversion from vec
261               
262                double ud = (Cud.ctrlaction(vec_1(-Isd)))(0);
263                double uq = (Cuq.ctrlaction(vec_1(Iqw-Isq)))(0);
264               
265                const double Ls0=0.003465; // inductance
266                const double Fmag0= 0.1989; // Magnetic??
267               
268                ud-=Ls0*ome*Iqw; // har
269                uq+=Fmag0*ome;
270               
271                double Um = sqrt(ud*ud+uq*uq);
272                double beta = atan(uq/ud);
273                if (ud<0) beta += M_PI;
274                beta +=the;
275               
276                vec uab(2);
277                uab(0)=Um*cos(beta);               // usx = usa
278                uab(1)=Um*sin(beta);    // usy
279/**/            //mykalm.setUab(uab(0),uab(1));         
280                return uab;
281        };
282};
283UIREGISTER(PMSM_PICtrl);
284
285/*************************************/
286/*                      LQ alpa-beta              */
287/*************************************/
288
289class PMSM_LQCtrl: public PMSMCtrl{
290public:
291/*
292PMSMCtrl:
293        double isa;
294        double isb;
295        double ome;
296        double the;
297        double Ww;
298*/     
299
300//PMSM parameters
301        const double a;
302        const double b;
303        const double c;
304        const double d;
305        const double e;
306       
307//input penalty
308        double r;
309       
310//time step
311        const double Dt;
312       
313//receding horizon
314        int rec_hor;
315       
316//system matrices
317        mat A; //5x5
318        mat B; //5x2
319        //mat C; //2x5
320        mat Q; //5x5
321        mat R; //2x2   
322       
323//control matrix
324        mat L;
325        vec uab;
326        vec icond;
327       
328//control maximum
329        double MAXu;
330        int MAXuflag;
331       
332//lqg controler class
333        LQG_universal lq;       
334       
335//prediction
336        vec p_isa, p_isb, p_ome, p_the;
337               
338        PMSM_LQCtrl():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
339                                r(0.005), Dt(0.000125), rec_hor(10),                                    //for r is a default value rewrited by pcp.txt file value
340                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2),
341                                uab(2), icond(6), MAXu(100.0), MAXuflag(0)      {                       
342                //set fix matrices elements & dimensions
343                A.zeros();
344                        A(0, 0) = A(1, 1) = a;
345                        A(2, 2) = d;
346                        A(2, 4) = d - 1;
347                        A(3, 2) = A(3, 4) = Dt;
348                        A(3, 3) = A(4, 4) = 1.0;
349                B.zeros();
350                        B(0, 0) = B(1, 1) = c;
351                //C.zeros();
352                //      C(0, 0) = C(1, 1) = 1.0;
353                Q.zeros();
354                        Q(2, 2) = 1.0;
355                R.zeros();
356                                                               
357        }
358               
359       
360        virtual vec ctrlaction(const itpp::vec& cond) {
361                PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
362               
363                int i;
364                lq.resetTime();
365               
366                //create prediction
367                /*p_isa.zeros();
368                p_isb.zeros();
369                p_ome.zeros();
370                p_the.zeros();
371                p_isa(0) = isa;
372                p_isb(0) = isb;
373                p_ome(0) = ome;
374                p_the(0) = the;
375               
376                for(i = 0; i < rec_hor-1; i++){
377                        p_isa(i+1) = a*p_isa(i) + b*p_ome(i)*sin(p_the(i)) + c*0;//uab(0); //use last used control
378                        p_isb(i+1) = a*p_isb(i) - b*p_ome(i)*cos(p_the(i)) + c*0;//uab(1);
379                        p_ome(i+1) = d*p_ome(i) + e*(p_isb(i)*cos(p_the(i)) - p_isa(i)*sin(p_the(i)));
380                        p_the(i+1) = p_the(i) + Dt*p_ome(i);
381                }*/
382               
383                        A(0, 2) = b*sin(the);
384                        //A(0, 3) = b*ome*cos(the);                     
385                        A(0, 4) = b*sin(the);
386                        A(1, 2) = -b*cos(the);
387                        //A(1, 3) = b*ome*sin(the);                     
388                        A(1, 4) = -b*cos(the);
389                        A(2, 0) = -e*sin(the);
390                        A(2, 1) = e*cos(the);   
391                        //A(2, 3) = -e*(isa*cos(the) + isb*sin(the));           
392                       
393                        lq.Models(0).A = A;             
394               
395                //create control matrix
396                for(i = rec_hor; i > 0; i--){           
397                        //set variable matrices elements (A matrix only)
398                        /*A(0, 2) = b*sin(p_the(i));
399                        A(0, 3) = b*(p_ome(i))*cos(p_the(i));
400                        A(0, 4) = b*sin(p_the(i));
401                        A(1, 2) = -b*cos(p_the(i));
402                        A(1, 3) = b*(p_ome(i))*sin(p_the(i));
403                        A(1, 4) = -b*cos(p_the(i));
404                        A(2, 0) = -e*sin(p_the(i));
405                        A(2, 1) = e*cos(p_the(i));
406                        A(2, 3) = -e*(p_isa(i)*cos(p_the(i)) + p_isb(i)*sin(p_the(i)));
407                       
408                        lq.Models(0).A = A;     */     
409                        lq.redesign();
410                }
411                lq.redesign();
412               
413                L = lq.getL();
414                icond(0) = isa;
415                icond(1) = isb;
416                icond(2) = ome - Ww;
417                icond(3) = the;
418                icond(4) = Ww;
419                icond(5) = 0;
420                vec tmp = L*icond;                 
421               
422                uab = tmp(0,1);         
423               
424                if(MAXuflag == 1){ //square cut off
425                        if(uab(0) > MAXu) uab(0) = MAXu;
426                        else if(uab(0) < -MAXu) uab(0) = -MAXu;
427                        if(uab(1) > MAXu) uab(1) = MAXu;
428                        else if(uab(1) < -MAXu) uab(1) = -MAXu;
429                }
430                else if(MAXuflag == 2){ //circular cut off
431                        double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1));
432                        double uangl = atan2(uab(1), uab(0));
433                        if (uampl > MAXu) uampl = MAXu;
434                        uab(0) = uampl*cos(uangl);
435                        uab(1) = uampl*sin(uangl);
436                }
437                //else{ //(MAXuflag == 0) //no cut off }
438               
439                return uab;
440        }
441       
442        void from_setting(const Setting &set){
443                PMSMCtrl::from_setting(set);
444                UI::get(r,set, "r", UI::optional);
445                UI::get(rec_hor,set, "h", UI::optional);
446                UI::get(MAXu,set, "MAXu", UI::optional);
447                UI::get(MAXuflag,set, "MAXuflag", UI::optional);
448        }
449
450        void validate(){
451                R(0,0)=R(1,1)=r;
452
453                p_isa.set_length(rec_hor+1);
454                p_isb.set_length(rec_hor+1);
455                p_ome.set_length(rec_hor+1);
456                p_the.set_length(rec_hor+1);
457
458                Array<quadraticfn> qloss(2);
459                qloss(0).Q.setCh(Q);
460                qloss(0).rv = RV("x", 5, 1);
461                qloss(1).Q.setCh(R);
462                qloss(1).rv = RV("u", 2, 0);           
463                lq.Losses = qloss;             
464
465                                //set lq
466                lq.rv = RV("u", 2, 0);                 
467                lq.set_rvc(RV("x", 5, 0));
468                lq.horizon = rec_hor;   
469               
470                Array<linfnEx> model(2);
471                model(0).A = A;
472                model(0).B = vec("0 0 0 0 0");
473                model(0).rv = RV("x", 5, 0);
474                model(0).rv_ret = RV("x", 5, 1);
475                model(1).A = B;
476                model(1).B = vec("0 0");
477                model(1).rv = RV("u", 2, 0);
478                model(1).rv_ret = RV("x", 5, 1);
479                lq.Models = model;
480               
481                lq.finalLoss.Q.setCh(Q);
482                lq.finalLoss.rv = RV("x", 5, 1);
483               
484                lq.validate();
485                                               
486                uab.zeros();
487
488        }
489};
490UIREGISTER(PMSM_LQCtrl);
491
492/*************************************/
493/*                      LQ d-q 1                          */
494/*************************************/
495
496class PMSM_LQCtrl_dq: public PMSMCtrl{
497public:
498/*
499PMSMCtrl:
500        double isa;
501        double isb;
502        double ome;
503        double the;
504        double Ww;
505*/     
506
507//PMSM parameters
508        const double a;
509        const double b;
510        const double c;
511        const double d;
512        const double e;
513       
514//input penalty
515        double r;
516        double rpd; //penalize differences u - u_old
517       
518//time step
519        const double Dt;
520       
521//receding horizon
522        int rec_hor;
523       
524//system matrices
525        mat A; //5x5
526        mat B; //5x2
527        //mat C; //2x5
528        mat Q; //5x5
529        mat R; //2x2
530        mat Rpd; //2x4
531       
532//control matrix
533        mat L;
534        vec uab;       
535        vec icondpd;
536        vec u_old;
537       
538//control maximum
539        double MAXu;
540        int MAXuflag;
541       
542//lqg controler class
543        LQG_universal lq;       
544       
545//prediction
546        vec p_isa, p_isb, p_ome, p_the;
547               
548        PMSM_LQCtrl_dq():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
549                                r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10),                                  //for r is a default value rewrited by pcp.txt file value
550                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4),
551                                uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0)  {                       
552                //set fix matrices elements & dimensions
553                A.zeros();
554                        A(0, 0) = A(1, 1) = a;
555                        A(1, 2) = A(1, 4) = -b;
556                        A(2, 1) = e;
557                        A(2, 2) = d;
558                        A(2, 4) = d - 1;
559                        A(3, 2) = A(3, 4) = Dt;
560                        A(3, 3) = A(4, 4) = 1.0;
561                B.zeros();
562                        B(0, 0) = B(1, 1) = c;         
563                Q.zeros();
564                        Q(2, 2) = 1.0;
565                R.zeros();
566                Rpd.zeros();
567                u_old.zeros();                                         
568        }
569               
570       
571        virtual vec ctrlaction(const itpp::vec& cond) {
572                PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
573                               
574                vec udq;
575                vec tmp;
576               
577                lq.resetTime();
578                               
579                L = lq.getL();
580               
581                icondpd(0) = isa*cos(the) + isb*sin(the);
582                icondpd(1) = isb*cos(the) - isa*sin(the);
583                icondpd(2) = ome - Ww;
584                icondpd(3) = the;
585                icondpd(4) = Ww;
586                icondpd(5) = u_old(0);
587                icondpd(6) = u_old(1);
588                icondpd(7) = 0; 
589                       
590                tmp = L*icondpd;               
591                                               
592                udq = tmp(0,1);
593               
594                //uab = udq; //set size
595                uab(0) = udq(0)*cos(the) - udq(1)*sin(the);
596                uab(1) = udq(1)*cos(the) + udq(0)*sin(the);
597               
598                if(MAXuflag == 1){ //square cut off
599                        if(uab(0) > MAXu) uab(0) = MAXu;
600                        else if(uab(0) < -MAXu) uab(0) = -MAXu;
601                        if(uab(1) > MAXu) uab(1) = MAXu;
602                        else if(uab(1) < -MAXu) uab(1) = -MAXu;
603                }
604                else if(MAXuflag == 2){ //circular cut off
605                        double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1));
606                        double uangl = atan2(uab(1), uab(0));
607                        if (uampl > MAXu) uampl = MAXu;
608                        uab(0) = uampl*cos(uangl);
609                        uab(1) = uampl*sin(uangl);
610                }
611                //else{ //(MAXuflag == 0) //no cut off }
612               
613                u_old = udq;
614               
615                return uab;
616        }
617       
618        void from_setting(const Setting &set){
619                PMSMCtrl::from_setting(set);
620                UI::get(r,set, "r", UI::optional);
621                UI::get(rec_hor,set, "h", UI::optional);
622                UI::get(MAXu,set, "MAXu", UI::optional);
623                UI::get(MAXuflag,set, "MAXuflag", UI::optional);
624                UI::get(rpd,set, "rpd", UI::optional);         
625        }
626
627        void validate(){
628                R(0, 0) = R(1, 1) = r;
629                Rpd(0, 0) = Rpd(1, 1) = rpd;
630                Rpd(0, 2) = Rpd(1, 3) = -rpd;
631                               
632                p_isa.set_length(rec_hor+1);
633                p_isb.set_length(rec_hor+1);
634                p_ome.set_length(rec_hor+1);
635                p_the.set_length(rec_hor+1);
636
637                Array<quadraticfn> qloss(3);
638                qloss(0).Q.setCh(Q);
639                qloss(0).rv = RV("x", 5, 1);           
640                qloss(1).Q.setCh(R);
641                qloss(1).rv = RV("u", 2, 0);
642                qloss(2).Q.setCh(Rpd);         
643                qloss(2).rv = RV("u", 2, 0);
644                qloss(2).rv.add(RV("u", 2, -1));               
645                lq.Losses = qloss;             
646
647                //set lq
648                lq.rv = RV("u", 2, 0); 
649                RV tmp = RV("x", 5, 0);
650                tmp.add(RV("u", 2, -1));
651                lq.set_rvc(tmp);               
652                lq.horizon = rec_hor;   
653               
654                Array<linfnEx> model(2);
655                model(0).A = A;
656                model(0).B = vec("0 0 0 0 0");
657                model(0).rv = RV("x", 5, 0);
658                model(0).rv_ret = RV("x", 5, 1);
659                model(1).A = B;
660                model(1).B = vec("0 0");
661                model(1).rv = RV("u", 2, 0);
662                model(1).rv_ret = RV("x", 5, 1);
663                lq.Models = model;
664               
665                lq.finalLoss.Q.setCh(Q);
666                lq.finalLoss.rv = RV("x", 5, 1);
667                               
668                lq.validate();
669                                               
670                uab.zeros();
671                               
672                //create control matrix
673                for(int i = rec_hor; i > 0; i--){                                                               
674                        lq.redesign();
675                }
676                lq.redesign();
677        }
678};
679UIREGISTER(PMSM_LQCtrl_dq);
680
681/*************************************/
682/*                      LQ d-q 2                          */
683/*************************************/
684
685class PMSM_LQCtrl_dq2: public PMSMCtrl{
686public:
687/*
688PMSMCtrl:
689        double isa;
690        double isb;
691        double ome;
692        double the;
693        double Ww;
694*/     
695
696//PMSM parameters
697        const double a;
698        const double b;
699        const double c;
700        const double d;
701        const double e;
702       
703//input penalty
704        double r;
705        double rpd; //penalize differences u - u_old
706       
707//time step
708        const double Dt;
709       
710//receding horizon
711        int rec_hor;
712       
713//system matrices
714        mat A; //5x5
715        mat B; //5x2
716        //mat C; //2x5
717        mat Q; //5x5
718        mat R; //2x2
719        mat Rpd; //2x4 
720       
721//control matrix
722        mat L;
723        vec uab,udq;
724        vec icondpd;
725        vec u_old;
726       
727//control maximum
728        double MAXu;
729        int MAXuflag;
730       
731//lqg controler class
732        LQG_universal lq;       
733       
734//
735//      vec p_isd, p_isq, p_ome, p_the;
736        double p_isd, p_isq;
737               
738        PMSM_LQCtrl_dq2():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
739                                r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10),                                  //for r is a default value rewrited by pcp.txt file value
740                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4),
741                                uab(2), udq(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0)  {                       
742                //set fix matrices elements & dimensions
743                A.zeros();
744                        A(0, 0) = A(1, 1) = a;
745                        A(2, 1) = e;
746                        A(2, 2) = d;
747                        A(2, 4) = d - 1;
748                        A(3, 2) = A(3, 4) = Dt;
749                        A(3, 3) = A(4, 4) = 1.0;
750                B.zeros();
751                        B(0, 0) = B(1, 1) = c;         
752                Q.zeros();
753                        Q(2, 2) = 1.0;
754                R.zeros();
755                Rpd.zeros();
756                u_old.zeros();                                         
757        }
758               
759       
760        virtual vec ctrlaction(const itpp::vec& cond) {
761                PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
762               
763                int i;
764                vec tmp;
765               
766                lq.resetTime();
767       
768                //create prediction
769                /*p_isd.zeros();
770                p_isq.zeros();
771                p_ome.zeros();
772                p_the.zeros();*/
773                p_isd = isa*cos(the) + isb*sin(the);//isa;
774                p_isq = isb*cos(the) - isa*sin(the);//isb;
775                /*p_ome(0) = ome;
776                p_the(0) = the;
777               
778                for(i = 0; i < rec_hor-1; i++){                 
779                        p_isd(i+1) = a*p_isd(i) + p_isq(i)*p_ome(i)*Dt; + c*0.5*(udq(0)+u_old(0));                     
780                        p_isq(i+1) = -p_isd(i)*p_ome(i)*Dt + a*p_isq(i) - b*p_ome(i); + c*0.5*(udq(1)+u_old(1));
781                        p_ome(i+1) = d*p_ome(i) + e*p_isq(i);
782                        p_the(i+1) = p_the(i) + Dt*p_ome(i);                   
783                }*/
784               
785                A(0, 1) = Dt*ome;
786                //A(0, 2) = Dt*p_isq;
787                //A(0, 4) = Dt*p_isq;
788                A(1, 0) = -Dt*ome;
789                A(1, 2) = /*-Dt*p_isd*/-b;
790                A(1, 4) = /*-Dt*p_isd*/-b;
791                       
792                lq.Models(0).A = A;     
793               
794                //create control matrix
795                for(i = rec_hor; i > 0; i--){           
796                        //set variable matrices elements (A matrix only)                       
797                        /*A(0, 1) = Dt*p_ome(i);
798                        A(0, 2) = Dt*p_isq(i);
799                        A(0, 4) = Dt*p_isq(i);
800                        A(1, 0) = -Dt*p_ome(i);
801                        A(1, 2) = -Dt*p_isd(i);
802                        A(1, 4) = -Dt*p_isd(i);
803                       
804                        lq.Models(0).A = A;     */     
805                        lq.redesign();
806                }
807                lq.redesign();
808               
809                L = lq.getL();
810       
811                icondpd(0) = isa*cos(the) + isb*sin(the);
812                icondpd(1) = isb*cos(the) - isa*sin(the);
813                icondpd(2) = ome - Ww;
814                icondpd(3) = the;
815                icondpd(4) = Ww;
816                icondpd(5) = u_old(0);
817                icondpd(6) = u_old(1);
818                icondpd(7) = 0;
819                       
820                tmp = L*icondpd;                       
821                               
822                udq = tmp(0,1);
823               
824                //uab = udq; //set size
825                uab(0) = udq(0)*cos(the) - udq(1)*sin(the);
826                uab(1) = udq(1)*cos(the) + udq(0)*sin(the);
827               
828                if(MAXuflag == 1){ //square cut off
829                        if(uab(0) > MAXu) uab(0) = MAXu;
830                        else if(uab(0) < -MAXu) uab(0) = -MAXu;
831                        if(uab(1) > MAXu) uab(1) = MAXu;
832                        else if(uab(1) < -MAXu) uab(1) = -MAXu;
833                }
834                else if(MAXuflag == 2){ //circular cut off
835                        double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1));
836                        double uangl = atan2(uab(1), uab(0));
837                        if (uampl > MAXu) uampl = MAXu;
838                        uab(0) = uampl*cos(uangl);
839                        uab(1) = uampl*sin(uangl);
840                }
841                //else{ //(MAXuflag == 0) //no cut off }
842               
843                u_old = udq;
844       
845                return uab;
846        }
847       
848        void from_setting(const Setting &set){
849                PMSMCtrl::from_setting(set);
850                UI::get(r,set, "r", UI::optional);
851                UI::get(rec_hor,set, "h", UI::optional);
852                UI::get(MAXu,set, "MAXu", UI::optional);
853                UI::get(MAXuflag,set, "MAXuflag", UI::optional);
854                UI::get(rpd,set, "rpd", UI::optional);
855        }
856
857        void validate(){
858                R(0, 0) = R(1, 1) = r;
859                Rpd(0, 0) = Rpd(1, 1) = rpd;           
860                Rpd(0, 2) = Rpd(1, 3) = -rpd;           
861               
862                /*p_isd.set_length(rec_hor+1);
863                p_isq.set_length(rec_hor+1);
864                p_ome.set_length(rec_hor+1);
865                p_the.set_length(rec_hor+1);*/
866
867                Array<quadraticfn> qloss(3);
868                qloss(0).Q.setCh(Q);
869                qloss(0).rv = RV("x", 5, 1);           
870                qloss(1).Q.setCh(R);
871                qloss(1).rv = RV("u", 2, 0);
872                qloss(2).Q.setCh(Rpd);
873                qloss(2).rv = RV("u", 2, 0);
874                qloss(2).rv.add(RV("u", 2, -1));               
875                lq.Losses = qloss;             
876
877                //set lq
878                lq.rv = RV("u", 2, 0);         
879                RV tmp = RV("x", 5, 0);
880                tmp.add(RV("u", 2, -1));       
881                lq.set_rvc(tmp);
882               
883                lq.horizon = rec_hor;   
884               
885                Array<linfnEx> model(2);
886                model(0).A = A;
887                model(0).B = vec("0 0 0 0 0");
888                model(0).rv = RV("x", 5, 0);
889                model(0).rv_ret = RV("x", 5, 1);
890                model(1).A = B;
891                model(1).B = vec("0 0");
892                model(1).rv = RV("u", 2, 0);
893                model(1).rv_ret = RV("x", 5, 1);
894                lq.Models = model;
895               
896                lq.finalLoss.Q.setCh(Q);
897                lq.finalLoss.rv = RV("x", 5, 1);
898                               
899                lq.validate();
900                                               
901                uab.zeros();
902                udq.zeros();           
903        }
904};
905UIREGISTER(PMSM_LQCtrl_dq2);
906
907/*************************************/
908/*              LQ d-q Bicriterial         */
909/*************************************/
910
911class PMSM_LQCtrl_bic: public PMSMCtrl{
912public:
913/*
914PMSMCtrl:
915        double isa;
916        double isb;
917        double ome;
918        double the;
919        double Ww;
920*/     
921
922//PMSM parameters
923        const double a;
924        const double b;
925        const double c;
926        const double d;
927        const double e;
928       
929//input penalty
930        double r;
931        double rpd; //penalize differences u - u_old
932       
933//time step
934        const double Dt;
935       
936//receding horizon
937        int rec_hor;
938       
939//system matrices
940        mat A; //5x5
941        mat B; //5x2
942        //mat C; //2x5
943        mat Q; //5x5
944        mat R; //2x2
945        mat Rpd; //2x4 
946       
947//control matrix
948        mat L;
949        vec uab,udq;
950        vec icondpd;
951        vec u_old;
952       
953//control maximum
954        double MAXu;
955        int MAXuflag;
956       
957//lqg controler class
958        LQG_universal lq;       
959       
960//pomega
961        //double Pome;
962        double qqq;
963        vec bcb;
964        double bcbv;
965       
966//
967//      vec p_isd, p_isq, p_ome, p_the;
968        double p_isd, p_isq;
969               
970        PMSM_LQCtrl_bic():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
971                                r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10),                                  //for r is a default value rewrited by pcp.txt file value
972                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), //qqq(0.0),
973                                uab(2), udq(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0), bcb(2), bcbv(0.0)       {                       
974                //set fix matrices elements & dimensions
975                A.zeros();
976                        A(0, 0) = A(1, 1) = a;
977                        A(2, 1) = e;
978                        A(2, 2) = d;
979                        A(2, 4) = d - 1;
980                        A(3, 2) = A(3, 4) = Dt;
981                        A(3, 3) = A(4, 4) = 1.0;
982                        //A(5, 5) = 1.0;
983                B.zeros();
984                        B(0, 0) = B(1, 1) = c;         
985                Q.zeros();
986                        Q(2, 2) = 1.0;                 
987                R.zeros();
988                Rpd.zeros();
989                u_old.zeros();         
990               
991                //Pome = 10.0; 
992                                       
993        }
994               
995       
996        virtual vec ctrlaction(const itpp::vec& cond) {
997                PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
998               
999                int i;
1000                vec tmp;
1001               
1002                lq.resetTime();
1003       
1004                //create prediction             
1005                p_isd = isa*cos(the) + isb*sin(the);//isa;
1006                p_isq = isb*cos(the) - isa*sin(the);//isb;
1007                               
1008                A(0, 1) = Dt*ome;
1009                //A(0, 2) = Dt*p_isq;
1010                //A(0, 4) = Dt*p_isq;
1011                A(1, 0) = -Dt*ome;
1012                A(1, 2) = /*-Dt*p_isd*/-b;
1013                A(1, 4) = /*-Dt*p_isd*/-b;
1014                       
1015                lq.Models(0).A = A;     
1016               
1017                //create control matrix
1018                for(i = rec_hor; i > 0; i--){                                   
1019                        lq.redesign();
1020                }
1021                lq.redesign();
1022               
1023                L = lq.getL();
1024       
1025                icondpd(0) = isa*cos(the) + isb*sin(the);
1026                icondpd(1) = isb*cos(the) - isa*sin(the);
1027                icondpd(2) = ome - Ww;
1028                icondpd(3) = the;
1029                icondpd(4) = Ww;
1030                icondpd(5) = u_old(0);
1031                icondpd(6) = u_old(1);
1032                //icondpd(7) = sqrt(Pome);
1033                icondpd(7) = 0;
1034                       
1035                tmp = L*icondpd;                       
1036                               
1037                udq = tmp(0,1);//(1.0 + abs(Pome/ome));
1038               
1039                //bicriterial
1040                udq += sign(ome)*bcb;
1041               
1042                //uab = udq; //set size
1043                uab(0) = udq(0)*cos(the) - udq(1)*sin(the);
1044                uab(1) = udq(1)*cos(the) + udq(0)*sin(the);
1045               
1046                if(MAXuflag == 1){ //square cut off
1047                        if(uab(0) > MAXu) uab(0) = MAXu;
1048                        else if(uab(0) < -MAXu) uab(0) = -MAXu;
1049                        if(uab(1) > MAXu) uab(1) = MAXu;
1050                        else if(uab(1) < -MAXu) uab(1) = -MAXu;
1051                }
1052                else if(MAXuflag == 2){ //circular cut off
1053                        double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1));
1054                        double uangl = atan2(uab(1), uab(0));
1055                        if (uampl > MAXu) uampl = MAXu;
1056                        uab(0) = uampl*cos(uangl);
1057                        uab(1) = uampl*sin(uangl);
1058                }
1059                //else{ //(MAXuflag == 0) //no cut off }
1060               
1061                //variance Pome evolution
1062                //Pome *= 1.0/(1.0+abs(Pome/ome));
1063               
1064                u_old = udq;
1065       
1066                return uab;
1067        }
1068       
1069        void from_setting(const Setting &set){
1070                PMSMCtrl::from_setting(set);
1071                UI::get(r,set, "r", UI::optional);
1072                UI::get(rec_hor,set, "h", UI::optional);
1073                UI::get(MAXu,set, "MAXu", UI::optional);
1074                UI::get(MAXuflag,set, "MAXuflag", UI::optional);
1075                UI::get(rpd,set, "rpd", UI::optional);
1076                //UI::get(qqq,set, "qqq", UI::optional);
1077                UI::get(bcbv,set, "bcbv", UI::optional);
1078        }
1079
1080        void validate(){
1081                R(0, 0) = R(1, 1) = r;
1082                Rpd(0, 0) = Rpd(1, 1) = rpd;           
1083                Rpd(0, 2) = Rpd(1, 3) = -rpd;   
1084                //Q(5, 5) = qqq;       
1085               
1086                //bicriterial
1087                bcb(0) = -bcbv;
1088                bcb(1) = bcbv;
1089               
1090                /*p_isd.set_length(rec_hor+1);
1091                p_isq.set_length(rec_hor+1);
1092                p_ome.set_length(rec_hor+1);
1093                p_the.set_length(rec_hor+1);*/
1094
1095                Array<quadraticfn> qloss(3);
1096                qloss(0).Q.setCh(Q);
1097                qloss(0).rv = RV("x", 5, 1);           
1098                qloss(1).Q.setCh(R);
1099                qloss(1).rv = RV("u", 2, 0);
1100                qloss(2).Q.setCh(Rpd);
1101                qloss(2).rv = RV("u", 2, 0);
1102                qloss(2).rv.add(RV("u", 2, -1));               
1103                lq.Losses = qloss;             
1104
1105                //set lq
1106                lq.rv = RV("u", 2, 0);         
1107                RV tmp = RV("x", 5, 0);
1108                tmp.add(RV("u", 2, -1));       
1109                lq.set_rvc(tmp);
1110               
1111                lq.horizon = rec_hor;   
1112               
1113                Array<linfnEx> model(2);
1114                model(0).A = A;
1115                model(0).B = vec("0 0 0 0 0");
1116                model(0).rv = RV("x", 5, 0);
1117                model(0).rv_ret = RV("x", 5, 1);
1118                model(1).A = B;
1119                model(1).B = vec("0 0");
1120                model(1).rv = RV("u", 2, 0);
1121                model(1).rv_ret = RV("x", 5, 1);
1122                lq.Models = model;
1123               
1124                lq.finalLoss.Q.setCh(Q);
1125                lq.finalLoss.rv = RV("x", 5, 1);
1126                               
1127                lq.validate();
1128                                               
1129                uab.zeros();
1130                udq.zeros();           
1131        }
1132};
1133UIREGISTER(PMSM_LQCtrl_bic);
1134
1135/*************************************/
1136/*                      LQ d-q 1 bicriterial 2*/
1137/*************************************/
1138
1139class PMSM_LQCtrl_bic2: public PMSMCtrl{
1140public:
1141/*
1142PMSMCtrl:
1143        double isa;
1144        double isb;
1145        double ome;
1146        double the;
1147        double Ww;
1148*/     
1149
1150//PMSM parameters
1151        const double a;
1152        const double b;
1153        const double c;
1154        const double d;
1155        const double e;
1156       
1157//input penalty
1158        double r;
1159        double rpd; //penalize differences u - u_old
1160       
1161//time step
1162        const double Dt;
1163       
1164//receding horizon
1165        int rec_hor;
1166       
1167//system matrices
1168        mat A; //5x5
1169        mat B; //5x2
1170        //mat C; //2x5
1171        mat Q; //5x5
1172        mat R; //2x2
1173        mat Rpd; //2x4
1174       
1175//control matrix
1176        mat L;
1177        vec uab;       
1178        vec icondpd;
1179        vec u_old;
1180       
1181//control maximum
1182        double MAXu;
1183        int MAXuflag;
1184       
1185//lqg controler class
1186        LQG_universal lq;       
1187       
1188//prediction
1189        vec p_isa, p_isb, p_ome, p_the;
1190       
1191//bicrit param
1192        double bcbv;
1193       
1194       
1195//pi version   
1196        PI_ctrl Cwq;
1197        PI_ctrl Cuq;
1198        PI_ctrl Cud;
1199       
1200       
1201               
1202        PMSM_LQCtrl_bic2():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
1203                                r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10),                                  //for r is a default value rewrited by pcp.txt file value
1204                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4),
1205                                uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0),
1206                                /*Cwq(3.0, 3.0*0.000125/0.1, -1200, 1200),
1207                                Cuq(20.0, 20.0*0.000125/0.005, -1200, 1200),
1208                                Cud(20.0, 20.0*0.000125/0.005, -1200, 1200)*/
1209                                Cwq(10.0, 0.0005, -1200, 1200), 
1210                                Cuq(0.4, 0.0005, -1200, 1200),
1211                                Cud(0.4, 0.0005, -1200, 1200)   {                       
1212                //set fix matrices elements & dimensions
1213                A.zeros();
1214                        A(0, 0) = A(1, 1) = a;
1215                        A(1, 2) = A(1, 4) = -b;
1216                        A(2, 1) = e;
1217                        A(2, 2) = d;
1218                        A(2, 4) = d - 1;
1219                        A(3, 2) = A(3, 4) = Dt;
1220                        A(3, 3) = A(4, 4) = 1.0;
1221                B.zeros();
1222                        B(0, 0) = B(1, 1) = c;         
1223                Q.zeros();
1224                        Q(2, 2) = 1.0;
1225                R.zeros();
1226                Rpd.zeros();
1227                u_old.zeros();                                         
1228        }
1229               
1230       
1231        virtual vec ctrlaction(const itpp::vec& cond) {
1232                PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
1233                               
1234                vec udq;
1235                vec tmp;
1236               
1237                lq.resetTime();
1238                               
1239                L = lq.getL();
1240               
1241                icondpd(0) = isa*cos(the) + isb*sin(the);
1242                icondpd(1) = isb*cos(the) - isa*sin(the);
1243                icondpd(2) = ome - Ww;
1244                icondpd(3) = the;
1245                icondpd(4) = Ww;
1246                icondpd(5) = u_old(0);
1247                icondpd(6) = u_old(1);
1248                icondpd(7) = 0; 
1249                       
1250                tmp = L*icondpd;               
1251                                               
1252                udq = tmp(0,1);
1253               
1254               
1255               
1256                double Isd = isa*cos(the)+isb*sin(the);
1257                double Isq = isb*cos(the)-isa*sin(the);
1258               
1259                double Iqw=(Cwq.ctrlaction(vec_1(Ww-ome)))(0); // conversion from vec
1260               
1261                udq(0) = (Cud.ctrlaction(vec_1(-Isd)))(0);
1262                udq(1) = (Cuq.ctrlaction(vec_1(Iqw-Isq)))(0);
1263               
1264                const double Ls0=0.003465; // inductance
1265                const double Fmag0= 0.1989; // Magnetic??
1266               
1267                udq(0)-=Ls0*ome*Iqw; // har
1268                udq(1)+=Fmag0*ome;     
1269               
1270               
1271               
1272                //bicriterial
1273                double omerand = ome;// + sqrt(5.0e-6)*randn();
1274                udq(0) -= sign(omerand)*bcbv;
1275                udq(1) += sign(omerand)*bcbv;   
1276               
1277                               
1278                //uab = udq; //set size
1279                uab(0) = udq(0)*cos(the) - udq(1)*sin(the);
1280                uab(1) = udq(1)*cos(the) + udq(0)*sin(the);
1281               
1282                if(MAXuflag == 1){ //square cut off
1283                        if(uab(0) > MAXu) uab(0) = MAXu;
1284                        else if(uab(0) < -MAXu) uab(0) = -MAXu;
1285                        if(uab(1) > MAXu) uab(1) = MAXu;
1286                        else if(uab(1) < -MAXu) uab(1) = -MAXu;
1287                }
1288                else if(MAXuflag == 2){ //circular cut off
1289                        double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1));
1290                        double uangl = atan2(uab(1), uab(0));
1291                        if (uampl > MAXu) uampl = MAXu;
1292                        uab(0) = uampl*cos(uangl);
1293                        uab(1) = uampl*sin(uangl);
1294                }
1295                //else{ //(MAXuflag == 0) //no cut off }
1296               
1297                u_old = udq;
1298               
1299                return uab;                     
1300        }
1301       
1302        void from_setting(const Setting &set){
1303                PMSMCtrl::from_setting(set);
1304                UI::get(r,set, "r", UI::optional);
1305                UI::get(rec_hor,set, "h", UI::optional);
1306                UI::get(MAXu,set, "MAXu", UI::optional);
1307                UI::get(MAXuflag,set, "MAXuflag", UI::optional);
1308                UI::get(rpd,set, "rpd", UI::optional); 
1309                UI::get(bcbv,set, "bcbv", UI::optional);       
1310        }
1311
1312        void validate(){
1313                R(0, 0) = R(1, 1) = r;
1314                Rpd(0, 0) = Rpd(1, 1) = rpd;
1315                Rpd(0, 2) = Rpd(1, 3) = -rpd;
1316                               
1317                p_isa.set_length(rec_hor+1);
1318                p_isb.set_length(rec_hor+1);
1319                p_ome.set_length(rec_hor+1);
1320                p_the.set_length(rec_hor+1);
1321
1322                Array<quadraticfn> qloss(3);
1323                qloss(0).Q.setCh(Q);
1324                qloss(0).rv = RV("x", 5, 1);           
1325                qloss(1).Q.setCh(R);
1326                qloss(1).rv = RV("u", 2, 0);
1327                qloss(2).Q.setCh(Rpd);         
1328                qloss(2).rv = RV("u", 2, 0);
1329                qloss(2).rv.add(RV("u", 2, -1));               
1330                lq.Losses = qloss;             
1331
1332                //set lq
1333                lq.rv = RV("u", 2, 0); 
1334                RV tmp = RV("x", 5, 0);
1335                tmp.add(RV("u", 2, -1));
1336                lq.set_rvc(tmp);               
1337                lq.horizon = rec_hor;   
1338               
1339                Array<linfnEx> model(2);
1340                model(0).A = A;
1341                model(0).B = vec("0 0 0 0 0");
1342                model(0).rv = RV("x", 5, 0);
1343                model(0).rv_ret = RV("x", 5, 1);
1344                model(1).A = B;
1345                model(1).B = vec("0 0");
1346                model(1).rv = RV("u", 2, 0);
1347                model(1).rv_ret = RV("x", 5, 1);
1348                lq.Models = model;
1349               
1350                lq.finalLoss.Q.setCh(Q);
1351                lq.finalLoss.rv = RV("x", 5, 1);
1352                               
1353                lq.validate();
1354                                               
1355                uab.zeros();
1356                               
1357                //create control matrix
1358                for(int i = rec_hor; i > 0; i--){                                                               
1359                        lq.redesign();
1360                }
1361                lq.redesign();
1362        }
1363};
1364UIREGISTER(PMSM_LQCtrl_bic2);
1365
1366/*************************************/
1367/*                      PI bicriterial 3 s vice EKF pro urceni nejlepsi bikrit*/
1368/*************************************/
1369
1370class PMSM_LQCtrl_bic3: public PMSMCtrl{
1371        LOG_LEVEL(PMSM_LQCtrl_bic3, logModel);
1372public:
1373       
1374/*
1375PMSMCtrl:
1376        double isa;
1377        double isb;
1378        double ome;
1379        double the;
1380        double Ww;
1381*/     
1382
1383//PMSM parameters
1384        const double a;
1385        const double b;
1386        const double c;
1387        const double d;
1388        const double e;
1389       
1390//input penalty
1391        double r;
1392        double rpd; //penalize differences u - u_old
1393       
1394//time step
1395        const double Dt;
1396       
1397//receding horizon
1398        int rec_hor;
1399       
1400//system matrices
1401        mat A; //5x5
1402        mat B; //5x2
1403        //mat C; //2x5
1404        mat Q; //5x5
1405        mat R; //2x2
1406        mat Rpd; //2x4
1407       
1408//control matrix
1409        mat L;
1410        vec uab;       
1411        vec icondpd;
1412        vec u_old;
1413       
1414//control maximum
1415        double MAXu;
1416        int MAXuflag;
1417       
1418//lqg controler class
1419        LQG_universal lq;       
1420       
1421//prediction
1422        vec p_isa, p_isb, p_ome, p_the;
1423       
1424//bicrit param
1425        double bcbv;
1426       
1427       
1428//pi version   
1429        PI_ctrl Cwq;
1430        PI_ctrl Cuq;
1431        PI_ctrl Cud;
1432       
1433// 5KF
1434        mat Ptp, Kt, Ared, Cred, Qred, Rred;
1435        mat Pt1,Pt2,Pt3,Pt4,Pt5;
1436        vec varPth;
1437        int timeid;
1438        ofstream f;
1439
1440        int biver;
1441       
1442        double bcbv0;
1443       
1444        mat ovA, ovC, ovP, ovQ, ovR;
1445        double varome;
1446       
1447// inj
1448        double injkon, injome, injphi;
1449        int minindex;
1450               
1451        PMSM_LQCtrl_bic3():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
1452                                r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10),                                  //for r is a default value rewrited by pcp.txt file value
1453                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), biver(0),
1454                                uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0),
1455                                Ptp(2,2),Kt(2,2),Ared(2,2),Cred(2,2),Qred(2,2),Rred(2,2),
1456                                Pt1(2,2),Pt2(2,2),Pt3(2,2),Pt4(2,2),Pt5(2,2), varPth(5),
1457                                ovA(4,4),       ovC(2,4), ovP(4,4), ovQ(4,4), ovR(2,2),                 
1458                                /*Cwq(3.0, 3.0*0.000125/0.1, -1200, 1200),
1459                                Cuq(20.0, 20.0*0.000125/0.005, -1200, 1200),
1460                                Cud(20.0, 20.0*0.000125/0.005, -1200, 1200)*/
1461                                Cwq(10.0, 0.0005, -1200, 1200), 
1462                                Cuq(0.4, 0.0005, -1200, 1200),
1463                                Cud(0.4, 0.0005, -1200, 1200)   {                       
1464                //set fix matrices elements & dimensions
1465                A.zeros();
1466                        A(0, 0) = A(1, 1) = a;
1467                        A(1, 2) = A(1, 4) = -b;
1468                        A(2, 1) = e;
1469                        A(2, 2) = d;
1470                        A(2, 4) = d - 1;
1471                        A(3, 2) = A(3, 4) = Dt;
1472                        A(3, 3) = A(4, 4) = 1.0;
1473                B.zeros();
1474                        B(0, 0) = B(1, 1) = c;         
1475                Q.zeros();
1476                        Q(2, 2) = 1.0;
1477                R.zeros();
1478                Rpd.zeros();
1479                u_old.zeros(); 
1480               
1481                Ared(0,0) = d;
1482                Ared(1,0) = Dt;
1483                Ared(1,1) = 1.0;
1484                Qred(0,0) = 0.1;
1485                Qred(1,1) = 0.01;               
1486                Rred(0,0) = Rred(1,1) = 0.5; 
1487                Pt1(0,0) = Pt2(0,0) = Pt3(0,0) = Pt4(0,0) = Pt5(0,0) = 1.0;     
1488                Pt1(1,1) = Pt2(1,1) = Pt3(1,1) = Pt4(1,1) = Pt5(1,1) = 1.0;     
1489               
1490                timeid = 0;                     
1491                f.open("skf.dat", ios::out);
1492               
1493                injkon = injome = injphi = 0.0;
1494               
1495                srand(time(NULL));
1496               
1497                bcbv0 = bcbv;
1498               
1499                ovP=eye(4);
1500                ovA.zeros();
1501                ovC.zeros();
1502                ovQ.zeros();
1503                ovR.zeros();
1504                ovA(0,0) = ovA(1,1) = a;
1505                ovA(2,2) = d;
1506                ovA(3,3) = 1.0;
1507                ovA(3,2) = Dt;
1508                ovC(0,0) = ovC(1,1) = 1.0;
1509                ovR(0,0) = ovR(1,1) = 0.00001;
1510                ovQ(0,0) = ovQ(1,1) = 0.1;
1511                ovQ(2,2) = 0.1;
1512                ovQ(3,3) = 0.00001;
1513                varome = 1.0;
1514        }
1515       
1516        ~PMSM_LQCtrl_bic3(){
1517                f.close();
1518        }       
1519       
1520        virtual vec ctrlaction(const itpp::vec& cond) {
1521                PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
1522                                               
1523                vec udq(2);
1524                               
1525                double Isd = isa*cos(the)+isb*sin(the);
1526                double Isq = isb*cos(the)-isa*sin(the);
1527               
1528                double www = Ww;
1529                if(biver == 12){
1530                        www += (double)rand() / (double)RAND_MAX * 3.0 - 1.5;
1531                }
1532               
1533                double Iqw=(Cwq.ctrlaction(vec_1(www-ome)))(0); // conversion from vec
1534               
1535                udq(0) = (Cud.ctrlaction(vec_1(-Isd)))(0);
1536                udq(1) = (Cuq.ctrlaction(vec_1(Iqw-Isq)))(0);
1537               
1538                const double Ls0=0.003465; // inductance
1539                const double Fmag0= 0.1989; // Magnetic??
1540               
1541                udq(0)-=Ls0*ome*Iqw; // har
1542                udq(1)+=Fmag0*ome;     
1543               
1544               
1545               
1546                //bicriterial/////////////////////////////////////////////////////////////             
1547               
1548               
1549                //verze 1: signum
1550                if(biver == 1){
1551                        udq(0) -= sign(ome)*bcbv;
1552                        udq(1) += sign(ome)*bcbv;
1553                }
1554                //*/   
1555               
1556                //verze 2: casovy posun
1557                if(biver == 2){
1558                     double psi = (d*d - b*e)*ome + (a + d)*e*Isq - e*Dt*Isd*ome;
1559                     udq(1) += sign(psi)*bcbv;
1560                   
1561                     double ksi = d*psi + a*c*e*udq(1) - e*(a*b + b*d + a*Dt*(1+d)*Isd)*ome + e*(a*a - b*e - a*e*Dt*Isd)*Isq - e*Dt*Dt*(d*ome + e*Isq)*Isq*ome;
1562                     udq(0) -= sign(ksi)*bcbv;
1563          }
1564          //*/
1565         
1566          //verze 3: 3KFdq
1567          if((biver == 3)||(biver == 30)){   
1568                     double du_al = bcbv*cos(the);
1569                        double du_be = bcbv*sin(the);     
1570                        double ia = a*isa + b*ome*sin(the) + c*(uab(0) + du_al);
1571                        double ib = a*isb - b*ome*cos(the) + c*(uab(1) + du_be);   
1572                        double om = d*ome + e*(isb*cos(the) - isa*sin(the));
1573                        double th = the + Dt*ome;               
1574                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));
1575                        Cred(0,0) = b*sin(th);
1576                        Cred(0,1) = b*om*cos(th);
1577                        Cred(1,0) = -b*cos(th);
1578                        Cred(1,1) = b*om*sin(th);               
1579                        Ptp = Ared*Pt1*Ared.T() + Qred;
1580                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1581                        Pt1 = (eye(2) - Kt*Cred)*Ptp;
1582                        varPth(0) = Pt1(1,1);
1583                        if(biver == 30) varPth(0) = Pt1(0,0)*Pt1(1,1) - Pt1(0,1)*Pt1(1,0);             
1584               
1585                        ia = a*isa + b*ome*sin(the) + c*(uab(0) - du_al);
1586                        ib = a*isb - b*ome*cos(the) + c*(uab(1) - du_be);                               
1587                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1588                        Ptp = Ared*Pt2*Ared.T() + Qred;
1589                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1590                        Pt2 = (eye(2) - Kt*Cred)*Ptp;
1591                        varPth(1) = Pt2(1,1);
1592                        if(biver == 30) varPth(1) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0);
1593                               
1594                        ia = a*isa + b*ome*sin(the) + c*(uab(0));
1595                        ib = a*isb - b*ome*cos(the) + c*(uab(1));                               
1596                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1597                        Ptp = Ared*Pt5*Ared.T() + Qred;
1598                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1599                        Pt5 = (eye(2) - Kt*Cred)*Ptp;
1600                        varPth(2) = Pt5(1,1);
1601                        if(biver == 30) varPth(2) = Pt5(0,0)*Pt5(1,1) - Pt5(0,1)*Pt5(1,0);
1602                               
1603                        minindex = 0;
1604                        for(int i = 1; i < 3; i++){
1605                                if(varPth(i) < varPth(minindex)){                               
1606                                        minindex = i;
1607                                }
1608                        }
1609               
1610                        f << timeid << "\t" << minindex << endl;
1611                        timeid++;
1612               
1613                        switch(minindex){
1614                                case 0:
1615                                        udq(0) += bcbv;                         
1616                                        break;
1617                                case 1:
1618                                        udq(0) -= bcbv;                         
1619                                        break;                 
1620                                case 2:                         
1621                                        break;
1622                        }
1623                }
1624          //*/
1625         
1626          //verze 5: konst. v d
1627          if(biver == 5){
1628                udq(0) += 2*bcbv;
1629          }
1630          //*/
1631         
1632          //verze 6: injektaz v dq
1633          if(biver == 6){
1634                udq(0) += injkon*cos(injome*Dt*timeid + injphi);
1635                //udq(1) += injkon*cos(injome*Dt*timeid + injphi);
1636               
1637                timeid++;
1638          }
1639          //*/
1640         
1641          //verze 8: injektaz v dq
1642          if(biver == 8){
1643                udq(0) += injkon*cos(injome*Dt*timeid + injphi);
1644                udq(1) += injkon*sin(injome*Dt*timeid + injphi);
1645               
1646                timeid++;
1647          }
1648          //*/
1649         
1650          //uab = udq; //set size
1651                uab(0) = udq(0)*cos(the) - udq(1)*sin(the);
1652                uab(1) = udq(1)*cos(the) + udq(0)*sin(the);
1653         
1654          //verze 4: 5KF         
1655          if((biver == 4)||(biver == 40)){
1656               
1657               
1658                ovA(0,2) = b*sin(the);
1659                ovA(0,3) = b*ome*cos(the);
1660                ovA(1,2) = -b*cos(the);
1661                ovA(1,3) = b*ome*sin(the);
1662                ovA(2,0) = -e*sin(the);
1663                ovA(2,1) = e*cos(the);
1664                ovA(2,3) = -e*(isb*sin(the)+isa*cos(the));             
1665                mat ovPp = ovA*ovP*ovA.T() + ovQ;
1666                ovP = (eye(4) - ovPp*ovC.T()*inv(ovC*ovPp*ovC.T() + ovR )*ovC)*ovPp;
1667                varome = ovP(2,2);
1668               
1669                //bcbv = (sqrt( (ome)*(ome) + bcbv0*bcbv0 ) - abs(ome));
1670                //double tmp = (ome-Ww)*(ome-Ww) + bcbv0*bcbv0*e*e*c*c*0.5*(1.0+(varome-10.0)/(4.0));
1671                //if(tmp < 0.0) tmp = 0.0;
1672                //bcbv = (-abs(ome-Ww) + sqrt( tmp )) /e/c;
1673                         
1674                        double ia = a*isa + b*ome*sin(the) + c*(uab(0) + bcbv0);
1675                        double ib = a*isb - b*ome*cos(the) + c*(uab(1) + bcbv0);   
1676                        double om = d*ome + e*(isb*cos(the) - isa*sin(the));
1677                        double th = the + Dt*ome;               
1678                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));
1679                        Cred(0,0) = b*sin(th);
1680                        Cred(0,1) = b*om*cos(th);
1681                        Cred(1,0) = -b*cos(th);
1682                        Cred(1,1) = b*om*sin(th);               
1683                        Ptp = Ared*Pt1*Ared.T() + Qred;
1684                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1685                        Pt1 = (eye(2) - Kt*Cred)*Ptp;
1686                        varPth(0) = Pt1(1,1);
1687                        if(biver == 40) varPth(0) = Pt1(0,0)*Pt1(1,1) - Pt1(0,1)*Pt1(1,0);
1688               
1689                        ia = a*isa + b*ome*sin(the) + c*(uab(0) + bcbv0);
1690                        ib = a*isb - b*ome*cos(the) + c*(uab(1) - bcbv0);                               
1691                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1692                        Ptp = Ared*Pt2*Ared.T() + Qred;
1693                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1694                        Pt2 = (eye(2) - Kt*Cred)*Ptp;
1695                        varPth(1) = Pt2(1,1);
1696                        if(biver == 40) varPth(1) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0);
1697               
1698                        ia = a*isa + b*ome*sin(the) + c*(uab(0) - bcbv0);
1699                        ib = a*isb - b*ome*cos(the) + c*(uab(1) + bcbv0);                               
1700                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1701                        Ptp = Ared*Pt3*Ared.T() + Qred;
1702                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1703                        Pt3 = (eye(2) - Kt*Cred)*Ptp;
1704                        varPth(2) = Pt3(1,1);
1705                        if(biver == 40) varPth(2) = Pt3(0,0)*Pt3(1,1) - Pt3(0,1)*Pt3(1,0);
1706               
1707                        ia = a*isa + b*ome*sin(the) + c*(uab(0) - bcbv0);
1708                        ib = a*isb - b*ome*cos(the) + c*(uab(1) - bcbv0);                               
1709                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1710                        Ptp = Ared*Pt4*Ared.T() + Qred;
1711                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1712                        Pt4 = (eye(2) - Kt*Cred)*Ptp;
1713                        varPth(3) = Pt4(1,1);
1714                        if(biver == 40) varPth(3) = Pt4(0,0)*Pt4(1,1) - Pt4(0,1)*Pt4(1,0);
1715               
1716                        ia = a*isa + b*ome*sin(the) + c*(uab(0));
1717                        ib = a*isb - b*ome*cos(the) + c*(uab(1));                               
1718                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1719                        Ptp = Ared*Pt5*Ared.T() + Qred;
1720                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1721                        Pt5 = (eye(2) - Kt*Cred)*Ptp;
1722                        varPth(4) = Pt5(1,1);
1723                        if(biver == 40) varPth(4) = Pt5(0,0)*Pt5(1,1) - Pt5(0,1)*Pt5(1,0);
1724                               
1725                        minindex = 0;
1726                        for(int i = 1; i < 5; i++){
1727                                if(varPth(i) < varPth(minindex)){                               
1728                                        minindex = i;
1729                                }
1730                        }
1731               
1732                        f << timeid << "\t" << minindex << endl;
1733                        timeid++;
1734               
1735                        switch(minindex){
1736                                case 0:
1737                                        uab(0) += bcbv;
1738                                        uab(1) += bcbv;
1739                                        break;
1740                                case 1:
1741                                        uab(0) += bcbv;
1742                                        uab(1) -= bcbv;
1743                                        break;
1744                                case 2:
1745                                        uab(0) -= bcbv;
1746                                        uab(1) += bcbv;
1747                                        break;
1748                                case 3:
1749                                        uab(0) -= bcbv;
1750                                        uab(1) -= bcbv;
1751                                        break;
1752                                case 4:                         
1753                                        break;
1754                        }
1755                }
1756          //*/
1757         
1758          //verze 44: 5KF         
1759          if((biver == 44)){
1760                        double ia = a*isa + b*ome*sin(the) + c*(uab(0) + bcbv);
1761                        double ib = a*isb - b*ome*cos(the) + c*(uab(1) + bcbv);   
1762                        double om = d*ome + e*(isb*cos(the) - isa*sin(the));
1763                        double th = the + Dt*ome;               
1764                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));
1765                        Cred(0,0) = b*sin(th);
1766                        Cred(0,1) = b*om*cos(th);
1767                        Cred(1,0) = -b*cos(th);
1768                        Cred(1,1) = b*om*sin(th);               
1769                        Ptp = Ared*Pt1*Ared.T() + Qred;
1770                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);                       
1771                        Pt2 = (eye(2) - Kt*Cred)*Ptp;
1772                        varPth(0) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0);
1773               
1774                        ia = a*isa + b*ome*sin(the) + c*(uab(0) + bcbv);
1775                        ib = a*isb - b*ome*cos(the) + c*(uab(1) - bcbv);                               
1776                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1777                        Ptp = Ared*Pt1*Ared.T() + Qred;
1778                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1779                        Pt2 = (eye(2) - Kt*Cred)*Ptp;
1780                        varPth(0) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0);
1781               
1782                        ia = a*isa + b*ome*sin(the) + c*(uab(0) - bcbv);
1783                        ib = a*isb - b*ome*cos(the) + c*(uab(1) + bcbv);                               
1784                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1785                        Ptp = Ared*Pt1*Ared.T() + Qred;
1786                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1787                        Pt2 = (eye(2) - Kt*Cred)*Ptp;
1788                        varPth(0) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0);
1789               
1790                        ia = a*isa + b*ome*sin(the) + c*(uab(0) - bcbv);
1791                        ib = a*isb - b*ome*cos(the) + c*(uab(1) - bcbv);                               
1792                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1793                        Ptp = Ared*Pt1*Ared.T() + Qred;
1794                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1795                        Pt2 = (eye(2) - Kt*Cred)*Ptp;
1796                        varPth(0) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0);
1797               
1798                        ia = a*isa + b*ome*sin(the) + c*(uab(0));
1799                        ib = a*isb - b*ome*cos(the) + c*(uab(1));                               
1800                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1801                        Ptp = Ared*Pt1*Ared.T() + Qred;
1802                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1803                        Pt2 = (eye(2) - Kt*Cred)*Ptp;
1804                        varPth(0) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0);
1805                               
1806                        minindex = 0;
1807                        for(int i = 1; i < 5; i++){
1808                                if(varPth(i) < varPth(minindex)){                               
1809                                        minindex = i;
1810                                }
1811                        }
1812               
1813                        f << timeid << "\t" << minindex << endl;
1814                        timeid++;
1815               
1816                        switch(minindex){
1817                                case 0:
1818                                        uab(0) += bcbv;
1819                                        uab(1) += bcbv;
1820                                        break;
1821                                case 1:
1822                                        uab(0) += bcbv;
1823                                        uab(1) -= bcbv;
1824                                        break;
1825                                case 2:
1826                                        uab(0) -= bcbv;
1827                                        uab(1) += bcbv;
1828                                        break;
1829                                case 3:
1830                                        uab(0) -= bcbv;
1831                                        uab(1) -= bcbv;
1832                                        break;
1833                                case 4:                         
1834                                        break;
1835                        }
1836                       
1837                        ia = a*isa + b*ome*sin(the) + c*(uab(0));
1838                        ib = a*isb - b*ome*cos(the) + c*(uab(1));                               
1839                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1840                        Ptp = Ared*Pt1*Ared.T() + Qred;
1841                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1842                        Pt1 = (eye(2) - Kt*Cred)*Ptp;
1843                }
1844          //*/
1845         
1846          //verze 7: injektaz v alfa beta
1847          if(biver == 7){
1848                uab(0) += injkon*cos(injome*Dt*timeid + injphi);
1849                uab(1) += injkon*sin(injome*Dt*timeid + injphi);
1850               
1851                timeid++;
1852          }
1853         
1854          //verze 9: nahodne
1855          if(biver == 9){
1856                minindex = rand() % 5;
1857                switch(minindex){
1858                                case 0:
1859                                        uab(0) += bcbv;
1860                                        uab(1) += bcbv;
1861                                        break;
1862                                case 1:
1863                                        uab(0) += bcbv;
1864                                        uab(1) -= bcbv;
1865                                        break;
1866                                case 2:
1867                                        uab(0) -= bcbv;
1868                                        uab(1) += bcbv;
1869                                        break;
1870                                case 3:
1871                                        uab(0) -= bcbv;
1872                                        uab(1) -= bcbv;
1873                                        break;
1874                                case 4:                         
1875                                        break;
1876                        }
1877          }
1878         
1879          //verze 10: postupne
1880          if(biver == 10){
1881                minindex = timeid % 5;
1882                switch(minindex){
1883                                case 0:
1884                                        uab(0) += bcbv;
1885                                        uab(1) += bcbv;
1886                                        break;
1887                                case 1:
1888                                        uab(0) += bcbv;
1889                                        uab(1) -= bcbv;
1890                                        break;
1891                                case 2:
1892                                        uab(0) -= bcbv;
1893                                        uab(1) += bcbv;
1894                                        break;
1895                                case 3:
1896                                        uab(0) -= bcbv;
1897                                        uab(1) -= bcbv;
1898                                        break;
1899                                case 4:                         
1900                                        break;
1901                        }
1902                        timeid++;
1903          }
1904         
1905          //verze 11: inj. round
1906          if(biver == 11){
1907                double deltaua = itpp::round(sin(injome*Dt*timeid + injphi));
1908                double deltaub = itpp::round(cos(injome*Dt*timeid + injphi));
1909                uab(0) += injkon*deltaua;
1910                uab(1) += injkon*deltaub;                   
1911                   
1912                        if(deltaua*deltaub == 0) minindex = 4;
1913                        else if(deltaua > 0){
1914                                if(deltaub > 0) minindex = 0;
1915                                else minindex = 1;
1916                        }
1917                        else{
1918                                if(deltaub > 0) minindex = 2;
1919                                else minindex = 3;
1920                        }
1921                   
1922                        timeid++;   
1923                }
1924          //*/
1925               
1926                //////////////////////////////////////////////////////////////////////////
1927                               
1928               
1929               
1930                if(MAXuflag == 1){ //square cut off
1931                        if(uab(0) > MAXu) uab(0) = MAXu;
1932                        else if(uab(0) < -MAXu) uab(0) = -MAXu;
1933                        if(uab(1) > MAXu) uab(1) = MAXu;
1934                        else if(uab(1) < -MAXu) uab(1) = -MAXu;
1935                }
1936                else if(MAXuflag == 2){ //circular cut off
1937                        double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1));
1938                        double uangl = atan2(uab(1), uab(0));
1939                        if (uampl > MAXu) uampl = MAXu;
1940                        uab(0) = uampl*cos(uangl);
1941                        uab(1) = uampl*sin(uangl);
1942                }
1943                //else{ //(MAXuflag == 0) //no cut off }
1944               
1945                //u_old = udq;
1946               
1947                return uab;                     
1948        }
1949       
1950        void from_setting(const Setting &set){
1951                PMSMCtrl::from_setting(set);
1952                UI::get(r,set, "r", UI::optional);
1953                UI::get(rec_hor,set, "h", UI::optional);
1954                UI::get(MAXu,set, "MAXu", UI::optional);
1955                UI::get(MAXuflag,set, "MAXuflag", UI::optional);
1956                UI::get(rpd,set, "rpd", UI::optional); 
1957                UI::get(bcbv,set, "bcbv", UI::optional);       
1958                UI::get(biver,set, "biver", UI::optional);
1959                UI::get(injkon,set, "injkon", UI::optional);
1960                UI::get(injome,set, "injome", UI::optional);
1961                UI::get(injphi,set, "injphi", UI::optional);
1962               
1963                UI::get(Qred(0,0),set, "Qred00", UI::optional);
1964                UI::get(Qred(0,1),set, "Qred01", UI::optional);
1965                UI::get(Qred(1,0),set, "Qred10", UI::optional);
1966                UI::get(Qred(1,1),set, "Qred11", UI::optional);
1967                UI::get(Rred(0,0),set, "Rred00", UI::optional);
1968                UI::get(Rred(0,1),set, "Rred01", UI::optional);
1969                UI::get(Rred(1,0),set, "Rred10", UI::optional);
1970                UI::get(Rred(1,1),set, "Rred11", UI::optional);
1971               
1972                double Pi,Pd;
1973                Cwq.get_params(Pd,Pi);
1974                UI::get(Pi,set,"PIw_Pi",UI::optional);
1975                UI::get(Pd,set,"PIw_Pd",UI::optional);
1976                Cwq.set_params(Pd,Pi);
1977               
1978                Cud.get_params(Pd,Pi);
1979                UI::get(Pi,set,"PIu_Pi",UI::optional);
1980                UI::get(Pd,set,"PIu_Pd",UI::optional);
1981                Cud.set_params(Pd,Pi);
1982                Cuq.set_params(Pd,Pi);
1983               
1984                bcbv0 = bcbv;
1985        }
1986
1987        void validate(){
1988                R(0, 0) = R(1, 1) = r;
1989                Rpd(0, 0) = Rpd(1, 1) = rpd;
1990                Rpd(0, 2) = Rpd(1, 3) = -rpd;
1991                               
1992                p_isa.set_length(rec_hor+1);
1993                p_isb.set_length(rec_hor+1);
1994                p_ome.set_length(rec_hor+1);
1995                p_the.set_length(rec_hor+1);
1996
1997                Array<quadraticfn> qloss(3);
1998                qloss(0).Q.setCh(Q);
1999                qloss(0).rv = RV("x", 5, 1);           
2000                qloss(1).Q.setCh(R);
2001                qloss(1).rv = RV("u", 2, 0);
2002                qloss(2).Q.setCh(Rpd);         
2003                qloss(2).rv = RV("u", 2, 0);
2004                qloss(2).rv.add(RV("u", 2, -1));               
2005                lq.Losses = qloss;             
2006
2007                //set lq
2008                lq.rv = RV("u", 2, 0); 
2009                RV tmp = RV("x", 5, 0);
2010                tmp.add(RV("u", 2, -1));
2011                lq.set_rvc(tmp);               
2012                lq.horizon = rec_hor;   
2013               
2014                Array<linfnEx> model(2);
2015                model(0).A = A;
2016                model(0).B = vec("0 0 0 0 0");
2017                model(0).rv = RV("x", 5, 0);
2018                model(0).rv_ret = RV("x", 5, 1);
2019                model(1).A = B;
2020                model(1).B = vec("0 0");
2021                model(1).rv = RV("u", 2, 0);
2022                model(1).rv_ret = RV("x", 5, 1);
2023                lq.Models = model;
2024               
2025                lq.finalLoss.Q.setCh(Q);
2026                lq.finalLoss.rv = RV("x", 5, 1);
2027                               
2028                lq.validate();
2029                                               
2030                uab.zeros();
2031                               
2032                //create control matrix
2033                for(int i = rec_hor; i > 0; i--){                                                               
2034                        lq.redesign();
2035                }
2036                lq.redesign();
2037        }
2038       
2039        void log_register ( logger &L, const string &prefix ) {
2040                PMSMCtrl::log_register(L,prefix);
2041                L.add_vector ( log_level, logModel, RV ( 2), prefix );
2042        }
2043        void log_write() const{
2044                PMSMCtrl::log_write();
2045                vec tmp(2);
2046                tmp(0) = double(minindex);
2047                tmp(1) = double(bcbv);
2048//              tmp(1) = double(varome);
2049                log_level.store( logModel , tmp); 
2050               
2051        }
2052
2053};
2054UIREGISTER(PMSM_LQCtrl_bic3);
2055
2056
2057/*!@}*/
2058#endif //PMSM_CTR_H
Note: See TracBrowser for help on using the browser.