root/applications/pmsm/pmsm_ctrl.h @ 1390

Revision 1387, 48.4 kB (checked in by smidl, 13 years ago)

oprava simulator + log do bic3

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