root/applications/pmsm/pmsm_ctrl.h @ 1393

Revision 1391, 42.4 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// inj
1443        double injkon, injome, injphi;
1444        int minindex;
1445               
1446        PMSM_LQCtrl_bic3():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
1447                                r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10),                                  //for r is a default value rewrited by pcp.txt file value
1448                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), biver(0),
1449                                uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0),
1450                                Ptp(2,2),Kt(2,2),Ared(2,2),Cred(2,2),Qred(2,2),Rred(2,2),
1451                                Pt1(2,2),Pt2(2,2),Pt3(2,2),Pt4(2,2),Pt5(2,2), varPth(5),                               
1452                                /*Cwq(3.0, 3.0*0.000125/0.1, -1200, 1200),
1453                                Cuq(20.0, 20.0*0.000125/0.005, -1200, 1200),
1454                                Cud(20.0, 20.0*0.000125/0.005, -1200, 1200)*/
1455                                Cwq(10.0, 0.0005, -1200, 1200), 
1456                                Cuq(0.4, 0.0005, -1200, 1200),
1457                                Cud(0.4, 0.0005, -1200, 1200)   {                       
1458                //set fix matrices elements & dimensions
1459                A.zeros();
1460                        A(0, 0) = A(1, 1) = a;
1461                        A(1, 2) = A(1, 4) = -b;
1462                        A(2, 1) = e;
1463                        A(2, 2) = d;
1464                        A(2, 4) = d - 1;
1465                        A(3, 2) = A(3, 4) = Dt;
1466                        A(3, 3) = A(4, 4) = 1.0;
1467                B.zeros();
1468                        B(0, 0) = B(1, 1) = c;         
1469                Q.zeros();
1470                        Q(2, 2) = 1.0;
1471                R.zeros();
1472                Rpd.zeros();
1473                u_old.zeros(); 
1474               
1475                Ared(0,0) = d;
1476                Ared(1,0) = Dt;
1477                Ared(1,1) = 1.0;
1478                Qred(0,0) = 0.1;
1479                Qred(1,1) = 0.01;               
1480                Rred(0,0) = Rred(1,1) = 0.5; 
1481                Pt1(0,0) = Pt2(0,0) = Pt3(0,0) = Pt4(0,0) = Pt5(0,0) = 1.0;     
1482                Pt1(1,1) = Pt2(1,1) = Pt3(1,1) = Pt4(1,1) = Pt5(1,1) = 1.0;     
1483               
1484                timeid = 0;                     
1485                f.open("skf.dat", ios::out);
1486               
1487                injkon = injome = injphi = 0.0;
1488               
1489                srand(time(NULL));
1490        }
1491       
1492        ~PMSM_LQCtrl_bic3(){
1493                f.close();
1494        }       
1495       
1496        virtual vec ctrlaction(const itpp::vec& cond) {
1497                PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
1498                               
1499                vec udq(2);
1500                               
1501                double Isd = isa*cos(the)+isb*sin(the);
1502                double Isq = isb*cos(the)-isa*sin(the);
1503               
1504                double Iqw=(Cwq.ctrlaction(vec_1(Ww-ome)))(0); // conversion from vec
1505               
1506                udq(0) = (Cud.ctrlaction(vec_1(-Isd)))(0);
1507                udq(1) = (Cuq.ctrlaction(vec_1(Iqw-Isq)))(0);
1508               
1509                const double Ls0=0.003465; // inductance
1510                const double Fmag0= 0.1989; // Magnetic??
1511               
1512                udq(0)-=Ls0*ome*Iqw; // har
1513                udq(1)+=Fmag0*ome;     
1514               
1515               
1516               
1517                //bicriterial/////////////////////////////////////////////////////////////
1518                //verze 1: signum
1519                if(biver == 1){
1520                        udq(0) -= sign(ome)*bcbv;
1521                        udq(1) += sign(ome)*bcbv;
1522                }
1523                //*/   
1524               
1525                //verze 2: casovy posun
1526                if(biver == 2){
1527                     double psi = (d*d - b*e)*ome + (a + d)*e*Isq - e*Dt*Isd*ome;
1528                     udq(1) += sign(psi)*bcbv;
1529                   
1530                     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;
1531                     udq(0) -= sign(ksi)*bcbv;
1532          }
1533          //*/
1534         
1535          //verze 3: 3KFdq
1536          if((biver == 3)||(biver == 30)){   
1537                     double du_al = bcbv*cos(the);
1538                        double du_be = bcbv*sin(the);     
1539                        double ia = a*isa + b*ome*sin(the) + c*(uab(0) + du_al);
1540                        double ib = a*isb - b*ome*cos(the) + c*(uab(1) + du_be);   
1541                        double om = d*ome + e*(isb*cos(the) - isa*sin(the));
1542                        double th = the + Dt*ome;               
1543                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));
1544                        Cred(0,0) = b*sin(th);
1545                        Cred(0,1) = b*om*cos(th);
1546                        Cred(1,0) = -b*cos(th);
1547                        Cred(1,1) = b*om*sin(th);               
1548                        Ptp = Ared*Pt1*Ared.T() + Qred;
1549                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1550                        Pt1 = (eye(2) - Kt*Cred)*Ptp;
1551                        varPth(0) = Pt1(1,1);
1552                        if(biver == 30) varPth(0) = Pt1(0,0)*Pt1(1,1) - Pt1(0,1)*Pt1(1,0);             
1553               
1554                        ia = a*isa + b*ome*sin(the) + c*(uab(0) - du_al);
1555                        ib = a*isb - b*ome*cos(the) + c*(uab(1) - du_be);                               
1556                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1557                        Ptp = Ared*Pt2*Ared.T() + Qred;
1558                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1559                        Pt2 = (eye(2) - Kt*Cred)*Ptp;
1560                        varPth(1) = Pt2(1,1);
1561                        if(biver == 30) varPth(1) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0);
1562                               
1563                        ia = a*isa + b*ome*sin(the) + c*(uab(0));
1564                        ib = a*isb - b*ome*cos(the) + c*(uab(1));                               
1565                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1566                        Ptp = Ared*Pt5*Ared.T() + Qred;
1567                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1568                        Pt5 = (eye(2) - Kt*Cred)*Ptp;
1569                        varPth(2) = Pt5(1,1);
1570                        if(biver == 30) varPth(2) = Pt5(0,0)*Pt5(1,1) - Pt5(0,1)*Pt5(1,0);
1571                               
1572                        minindex = 0;
1573                        for(int i = 1; i < 3; i++){
1574                                if(varPth(i) < varPth(minindex)){                               
1575                                        minindex = i;
1576                                }
1577                        }
1578               
1579                        f << timeid << "\t" << minindex << endl;
1580                        timeid++;
1581               
1582                        switch(minindex){
1583                                case 0:
1584                                        udq(0) += bcbv;                         
1585                                        break;
1586                                case 1:
1587                                        udq(0) -= bcbv;                         
1588                                        break;                 
1589                                case 2:                         
1590                                        break;
1591                        }
1592                }
1593          //*/
1594         
1595          //verze 5: konst. v d
1596          if(biver == 5){
1597                udq(0) += 2*bcbv;
1598          }
1599          //*/
1600         
1601          //verze 6: injektaz v dq
1602          if(biver == 6){
1603                udq(0) += injkon*cos(injome*Dt*timeid + injphi);
1604                //udq(1) += injkon*cos(injome*Dt*timeid + injphi);
1605               
1606                timeid++;
1607          }
1608          //*/
1609         
1610          //verze 8: injektaz v dq
1611          if(biver == 8){
1612                udq(0) += injkon*cos(injome*Dt*timeid + injphi);
1613                udq(1) += injkon*sin(injome*Dt*timeid + injphi);
1614               
1615                timeid++;
1616          }
1617          //*/
1618         
1619          //uab = udq; //set size
1620                uab(0) = udq(0)*cos(the) - udq(1)*sin(the);
1621                uab(1) = udq(1)*cos(the) + udq(0)*sin(the);
1622         
1623          //verze 4: 5KF         
1624          if((biver == 4)||(biver == 40)){
1625                        double ia = a*isa + b*ome*sin(the) + c*(uab(0) + bcbv);
1626                        double ib = a*isb - b*ome*cos(the) + c*(uab(1) + bcbv);   
1627                        double om = d*ome + e*(isb*cos(the) - isa*sin(the));
1628                        double th = the + Dt*ome;               
1629                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));
1630                        Cred(0,0) = b*sin(th);
1631                        Cred(0,1) = b*om*cos(th);
1632                        Cred(1,0) = -b*cos(th);
1633                        Cred(1,1) = b*om*sin(th);               
1634                        Ptp = Ared*Pt1*Ared.T() + Qred;
1635                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1636                        Pt1 = (eye(2) - Kt*Cred)*Ptp;
1637                        varPth(0) = Pt1(1,1);
1638                        if(biver == 40) varPth(0) = Pt1(0,0)*Pt1(1,1) - Pt1(0,1)*Pt1(1,0);
1639               
1640                        ia = a*isa + b*ome*sin(the) + c*(uab(0) + bcbv);
1641                        ib = a*isb - b*ome*cos(the) + c*(uab(1) - bcbv);                               
1642                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1643                        Ptp = Ared*Pt2*Ared.T() + Qred;
1644                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1645                        Pt2 = (eye(2) - Kt*Cred)*Ptp;
1646                        varPth(1) = Pt2(1,1);
1647                        if(biver == 40) varPth(1) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0);
1648               
1649                        ia = a*isa + b*ome*sin(the) + c*(uab(0) - bcbv);
1650                        ib = a*isb - b*ome*cos(the) + c*(uab(1) + bcbv);                               
1651                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1652                        Ptp = Ared*Pt3*Ared.T() + Qred;
1653                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1654                        Pt3 = (eye(2) - Kt*Cred)*Ptp;
1655                        varPth(2) = Pt3(1,1);
1656                        if(biver == 40) varPth(2) = Pt3(0,0)*Pt3(1,1) - Pt3(0,1)*Pt3(1,0);
1657               
1658                        ia = a*isa + b*ome*sin(the) + c*(uab(0) - bcbv);
1659                        ib = a*isb - b*ome*cos(the) + c*(uab(1) - bcbv);                               
1660                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1661                        Ptp = Ared*Pt4*Ared.T() + Qred;
1662                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1663                        Pt4 = (eye(2) - Kt*Cred)*Ptp;
1664                        varPth(3) = Pt4(1,1);
1665                        if(biver == 40) varPth(3) = Pt4(0,0)*Pt4(1,1) - Pt4(0,1)*Pt4(1,0);
1666               
1667                        ia = a*isa + b*ome*sin(the) + c*(uab(0));
1668                        ib = a*isb - b*ome*cos(the) + c*(uab(1));                               
1669                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                               
1670                        Ptp = Ared*Pt5*Ared.T() + Qred;
1671                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
1672                        Pt5 = (eye(2) - Kt*Cred)*Ptp;
1673                        varPth(4) = Pt5(1,1);
1674                        if(biver == 40) varPth(4) = Pt5(0,0)*Pt5(1,1) - Pt5(0,1)*Pt5(1,0);
1675                               
1676                        minindex = 0;
1677                        for(int i = 1; i < 5; i++){
1678                                if(varPth(i) < varPth(minindex)){                               
1679                                        minindex = i;
1680                                }
1681                        }
1682               
1683                        f << timeid << "\t" << minindex << endl;
1684                        timeid++;
1685               
1686                        switch(minindex){
1687                                case 0:
1688                                        uab(0) += bcbv;
1689                                        uab(1) += bcbv;
1690                                        break;
1691                                case 1:
1692                                        uab(0) += bcbv;
1693                                        uab(1) -= bcbv;
1694                                        break;
1695                                case 2:
1696                                        uab(0) -= bcbv;
1697                                        uab(1) += bcbv;
1698                                        break;
1699                                case 3:
1700                                        uab(0) -= bcbv;
1701                                        uab(1) -= bcbv;
1702                                        break;
1703                                case 4:                         
1704                                        break;
1705                        }
1706                }
1707          //*/
1708         
1709          //verze 7: injektaz v alfa beta
1710          if(biver == 7){
1711                uab(0) += injkon*cos(injome*Dt*timeid + injphi);
1712                uab(1) += injkon*sin(injome*Dt*timeid + injphi);
1713               
1714                timeid++;
1715          }
1716         
1717          //verze 9: nahodne
1718          if(biver == 9){
1719                minindex = rand() % 5;
1720                switch(minindex){
1721                                case 0:
1722                                        uab(0) += bcbv;
1723                                        uab(1) += bcbv;
1724                                        break;
1725                                case 1:
1726                                        uab(0) += bcbv;
1727                                        uab(1) -= bcbv;
1728                                        break;
1729                                case 2:
1730                                        uab(0) -= bcbv;
1731                                        uab(1) += bcbv;
1732                                        break;
1733                                case 3:
1734                                        uab(0) -= bcbv;
1735                                        uab(1) -= bcbv;
1736                                        break;
1737                                case 4:                         
1738                                        break;
1739                        }
1740          }
1741         
1742          //verze 10: postupne
1743          if(biver == 10){
1744                minindex = timeid % 5;
1745                switch(minindex){
1746                                case 0:
1747                                        uab(0) += bcbv;
1748                                        uab(1) += bcbv;
1749                                        break;
1750                                case 1:
1751                                        uab(0) += bcbv;
1752                                        uab(1) -= bcbv;
1753                                        break;
1754                                case 2:
1755                                        uab(0) -= bcbv;
1756                                        uab(1) += bcbv;
1757                                        break;
1758                                case 3:
1759                                        uab(0) -= bcbv;
1760                                        uab(1) -= bcbv;
1761                                        break;
1762                                case 4:                         
1763                                        break;
1764                        }
1765                        timeid++;
1766          }
1767         
1768          //verze 11: inj. round
1769          if(biver == 11){
1770                double deltaua = itpp::round(sin(injome*Dt*timeid + injphi));
1771                double deltaub = itpp::round(cos(injome*Dt*timeid + injphi));
1772                uab(0) += injkon*deltaua;
1773                uab(1) += injkon*deltaub;                   
1774                   
1775                        if(deltaua*deltaub == 0) minindex = 4;
1776                        else if(deltaua > 0){
1777                                if(deltaub > 0) minindex = 0;
1778                                else minindex = 1;
1779                        }
1780                        else{
1781                                if(deltaub > 0) minindex = 2;
1782                                else minindex = 3;
1783                        }
1784                   
1785                        timeid++;   
1786                }
1787          //*/
1788               
1789                //////////////////////////////////////////////////////////////////////////
1790                               
1791               
1792               
1793                if(MAXuflag == 1){ //square cut off
1794                        if(uab(0) > MAXu) uab(0) = MAXu;
1795                        else if(uab(0) < -MAXu) uab(0) = -MAXu;
1796                        if(uab(1) > MAXu) uab(1) = MAXu;
1797                        else if(uab(1) < -MAXu) uab(1) = -MAXu;
1798                }
1799                else if(MAXuflag == 2){ //circular cut off
1800                        double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1));
1801                        double uangl = atan2(uab(1), uab(0));
1802                        if (uampl > MAXu) uampl = MAXu;
1803                        uab(0) = uampl*cos(uangl);
1804                        uab(1) = uampl*sin(uangl);
1805                }
1806                //else{ //(MAXuflag == 0) //no cut off }
1807               
1808                //u_old = udq;
1809               
1810                return uab;                     
1811        }
1812       
1813        void from_setting(const Setting &set){
1814                PMSMCtrl::from_setting(set);
1815                UI::get(r,set, "r", UI::optional);
1816                UI::get(rec_hor,set, "h", UI::optional);
1817                UI::get(MAXu,set, "MAXu", UI::optional);
1818                UI::get(MAXuflag,set, "MAXuflag", UI::optional);
1819                UI::get(rpd,set, "rpd", UI::optional); 
1820                UI::get(bcbv,set, "bcbv", UI::optional);       
1821                UI::get(biver,set, "biver", UI::optional);
1822                UI::get(injkon,set, "injkon", UI::optional);
1823                UI::get(injome,set, "injome", UI::optional);
1824                UI::get(injphi,set, "injphi", UI::optional);
1825               
1826                UI::get(Qred(0,0),set, "Qred00", UI::optional);
1827                UI::get(Qred(0,1),set, "Qred01", UI::optional);
1828                UI::get(Qred(1,0),set, "Qred10", UI::optional);
1829                UI::get(Qred(1,1),set, "Qred11", UI::optional);
1830                UI::get(Rred(0,0),set, "Rred00", UI::optional);
1831                UI::get(Rred(0,1),set, "Rred01", UI::optional);
1832                UI::get(Rred(1,0),set, "Rred10", UI::optional);
1833                UI::get(Rred(1,1),set, "Rred11", UI::optional);
1834               
1835                double Pi,Pd;
1836                Cwq.get_params(Pd,Pi);
1837                UI::get(Pi,set,"PIw_Pi",UI::optional);
1838                UI::get(Pd,set,"PIw_Pd",UI::optional);
1839                Cwq.set_params(Pd,Pi);
1840               
1841                Cud.get_params(Pd,Pi);
1842                UI::get(Pi,set,"PIu_Pi",UI::optional);
1843                UI::get(Pd,set,"PIu_Pd",UI::optional);
1844                Cud.set_params(Pd,Pi);
1845                Cuq.set_params(Pd,Pi);
1846
1847        }
1848
1849        void validate(){
1850                R(0, 0) = R(1, 1) = r;
1851                Rpd(0, 0) = Rpd(1, 1) = rpd;
1852                Rpd(0, 2) = Rpd(1, 3) = -rpd;
1853                               
1854                p_isa.set_length(rec_hor+1);
1855                p_isb.set_length(rec_hor+1);
1856                p_ome.set_length(rec_hor+1);
1857                p_the.set_length(rec_hor+1);
1858
1859                Array<quadraticfn> qloss(3);
1860                qloss(0).Q.setCh(Q);
1861                qloss(0).rv = RV("x", 5, 1);           
1862                qloss(1).Q.setCh(R);
1863                qloss(1).rv = RV("u", 2, 0);
1864                qloss(2).Q.setCh(Rpd);         
1865                qloss(2).rv = RV("u", 2, 0);
1866                qloss(2).rv.add(RV("u", 2, -1));               
1867                lq.Losses = qloss;             
1868
1869                //set lq
1870                lq.rv = RV("u", 2, 0); 
1871                RV tmp = RV("x", 5, 0);
1872                tmp.add(RV("u", 2, -1));
1873                lq.set_rvc(tmp);               
1874                lq.horizon = rec_hor;   
1875               
1876                Array<linfnEx> model(2);
1877                model(0).A = A;
1878                model(0).B = vec("0 0 0 0 0");
1879                model(0).rv = RV("x", 5, 0);
1880                model(0).rv_ret = RV("x", 5, 1);
1881                model(1).A = B;
1882                model(1).B = vec("0 0");
1883                model(1).rv = RV("u", 2, 0);
1884                model(1).rv_ret = RV("x", 5, 1);
1885                lq.Models = model;
1886               
1887                lq.finalLoss.Q.setCh(Q);
1888                lq.finalLoss.rv = RV("x", 5, 1);
1889                               
1890                lq.validate();
1891                                               
1892                uab.zeros();
1893                               
1894                //create control matrix
1895                for(int i = rec_hor; i > 0; i--){                                                               
1896                        lq.redesign();
1897                }
1898                lq.redesign();
1899        }
1900       
1901        void log_register ( logger &L, const string &prefix ) {
1902                PMSMCtrl::log_register(L,prefix);
1903                L.add_vector ( log_level, logModel, RV ( 1), prefix );
1904        }
1905        void log_write() const{
1906                PMSMCtrl::log_write();
1907                log_level.store( logModel , double(minindex)); 
1908               
1909        }
1910
1911};
1912UIREGISTER(PMSM_LQCtrl_bic3);
1913
1914
1915/*!@}*/
1916#endif //PMSM_CTR_H
Note: See TracBrowser for help on using the browser.