Context Navigation

pmsm_ctrl.h @ 1424

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

Note: See TracBrowser for help on using the browser.

Context Navigation

root/applications/pmsm/pmsm_ctrl.h @ 1424

Download in other formats: