Index: applications/pmsm/pmsm_ctrl.h
===================================================================
--- applications/pmsm/pmsm_ctrl.h (revision 1398)
+++ applications/pmsm/pmsm_ctrl.h (revision 1403)
@@ -9,4 +9,5 @@
 
 #include <fstream>
+#include <sstream>
 
 /*! \defgroup PMSM 
@@ -533,5 +534,5 @@
 	mat L;
 	vec uab;	
-	vec icondpd;
+	//vec icondpd;
 	vec u_old;
 	
@@ -549,5 +550,5 @@
 				r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10), 					//for r is a default value rewrited by pcp.txt file value
 				A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4),
-				uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0)	{			
+				uab(2), u_old(2), /*icondpd(8),*/ MAXu(100.0), MAXuflag(0)	{			
 		//set fix matrices elements & dimensions
 		A.zeros();
@@ -574,4 +575,5 @@
 		vec udq;
 		vec tmp;
+		vec icondpd(8);
 		
 		lq.resetTime();
@@ -722,5 +724,5 @@
 	mat L;
 	vec uab,udq;
-	vec icondpd;
+	//vec icondpd;
 	vec u_old;
 	
@@ -739,5 +741,5 @@
 				r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10), 					//for r is a default value rewrited by pcp.txt file value
 				A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4),
-				uab(2), udq(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0)	{			
+				uab(2), udq(2), u_old(2), /*icondpd(8),*/ MAXu(100.0), MAXuflag(0)	{			
 		//set fix matrices elements & dimensions
 		A.zeros();
@@ -763,4 +765,5 @@
 		int i;
 		vec tmp;
+		vec icondpd(8);
 		
 		lq.resetTime();
@@ -948,5 +951,5 @@
 	mat L;
 	vec uab,udq;
-	vec icondpd;
+	//vec icondpd;
 	vec u_old;
 	
@@ -971,5 +974,5 @@
 				r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10), 					//for r is a default value rewrited by pcp.txt file value
 				A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), //qqq(0.0),
-				uab(2), udq(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0), bcb(2), bcbv(0.0)	{			
+				uab(2), udq(2), u_old(2), /*icondpd(8),*/ MAXu(100.0), MAXuflag(0), bcb(2), bcbv(0.0)	{			
 		//set fix matrices elements & dimensions
 		A.zeros();
@@ -999,4 +1002,5 @@
 		int i;
 		vec tmp;
+		vec icondpd(8);
 		
 		lq.resetTime();
@@ -1176,5 +1180,5 @@
 	mat L;
 	vec uab;	
-	vec icondpd;
+	//vec icondpd;
 	vec u_old;
 	
@@ -1203,5 +1207,5 @@
 				r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10), 					//for r is a default value rewrited by pcp.txt file value
 				A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4),
-				uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0),
+				uab(2), u_old(2), /*icondpd(8),*/ MAXu(100.0), MAXuflag(0),
 				/*Cwq(3.0, 3.0*0.000125/0.1, -1200, 1200), 
 				Cuq(20.0, 20.0*0.000125/0.005, -1200, 1200),
@@ -1234,4 +1238,5 @@
 		vec udq;
 		vec tmp;
+		vec icondpd(8);
 		
 		lq.resetTime();
@@ -1409,5 +1414,5 @@
 	mat L;
 	vec uab;	
-	vec icondpd;
+	//vec icondpd;
 	vec u_old;
 	
@@ -1448,9 +1453,18 @@
 	double injkon, injome, injphi;
 	int minindex;
+	
+// grid
+	int gsize; //grid matrix size
+	mat qloss, dloss, vomeg, vthet, vdet; //quadratic & dual loss, omega & theta variances, variance mat det
+	double gstep; //grid step size
+	double wwold; // old reference value
+	double kvo; //dual loss koeficient
+	int cntr;
+	int mini, minj;
 		
 	PMSM_LQCtrl_bic3():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
 				r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10), 					//for r is a default value rewrited by pcp.txt file value
-				A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), biver(0),
-				uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0),
+				A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), /*biver(0),*/
+				uab(2), u_old(2), /*icondpd(8),*/ MAXu(100.0), MAXuflag(0),
 				Ptp(2,2),Kt(2,2),Ared(2,2),Cred(2,2),Qred(2,2),Rred(2,2),
 				Pt1(2,2),Pt2(2,2),Pt3(2,2),Pt4(2,2),Pt5(2,2), varPth(5),
@@ -1461,5 +1475,6 @@
 				Cwq(10.0, 0.0005, -1200, 1200), 
 				Cuq(0.4, 0.0005, -1200, 1200),
-				Cud(0.4, 0.0005, -1200, 1200)	{			
+				Cud(0.4, 0.0005, -1200, 1200),
+				gsize(21)	{			
 		//set fix matrices elements & dimensions
 		A.zeros();
@@ -1478,4 +1493,5 @@
 		Rpd.zeros();
 		u_old.zeros();	
+		uab.zeros();
 		
 		Ared(0,0) = d;
@@ -1484,8 +1500,9 @@
 		Qred(0,0) = 0.1;
 		Qred(1,1) = 0.01;		
-		Rred(0,0) = Rred(1,1) = 0.5; 
+		Rred(0,0) = Rred(1,1) = 0.05; 
 		Pt1(0,0) = Pt2(0,0) = Pt3(0,0) = Pt4(0,0) = Pt5(0,0) = 1.0;	
 		Pt1(1,1) = Pt2(1,1) = Pt3(1,1) = Pt4(1,1) = Pt5(1,1) = 1.0;	
 		
+		biver = 0;
 		timeid = 0;			
 		f.open("skf.dat", ios::out);
@@ -1507,9 +1524,20 @@
 		ovA(3,2) = Dt;
 		ovC(0,0) = ovC(1,1) = 1.0;
-		ovR(0,0) = ovR(1,1) = 0.00001;
+		ovR(0,0) = ovR(1,1) = 0.05;
 		ovQ(0,0) = ovQ(1,1) = 0.1;
-		ovQ(2,2) = 0.1;
-		ovQ(3,3) = 0.00001;
+		ovQ(2,2) = 0.01;
+		ovQ(3,3) = 0.0001;
 		varome = 1.0;
+		
+		qloss.set_size(gsize,gsize); 
+		dloss.set_size(gsize,gsize); 
+		vomeg.set_size(gsize,gsize);
+		vthet.set_size(gsize,gsize);
+		vdet.set_size(gsize,gsize);
+		
+		gstep = 0.5;
+		wwold = 0.0;
+		kvo = 0.00001;
+		cntr = 0;
 	}
 	
@@ -1656,5 +1684,5 @@
           	
           	
-          	ovA(0,2) = b*sin(the);
+          	/*ovA(0,2) = b*sin(the);
           	ovA(0,3) = b*ome*cos(the);
           	ovA(1,2) = -b*cos(the);
@@ -1665,5 +1693,5 @@
           	mat ovPp = ovA*ovP*ovA.T() + ovQ;
           	ovP = (eye(4) - ovPp*ovC.T()*inv(ovC*ovPp*ovC.T() + ovR )*ovC)*ovPp;
-          	varome = ovP(2,2);
+          	varome = ovP(2,2);*/
           	
           	//bcbv = (sqrt( (ome)*(ome) + bcbv0*bcbv0 ) - abs(ome));
@@ -1924,4 +1952,162 @@
           //*/
 		
+		//verze 13: grid
+          if(biver == 13){
+          	double ia,ib,om,th,ps;
+          	int hsize = floor(gsize/2);
+          	double dua, dub;
+          	double prww = 3.0*Ww - 2.0*wwold;
+          	int i,j;
+          	double omoo;
+          	double minloss = 1.0e300;
+          	mini=hsize;
+          	minj=hsize;
+          	mat ovPp;
+          	mat tmpP;
+          				
+			for(i = 0; i < gsize; i++){
+				for(j = 0; j < gsize; j++){
+					dua = (double)(i - hsize)*gstep;
+					dub = (double)(j - hsize)*gstep;					
+					
+					ia = a*isa + b*ome*sin(the) + c*(uab(0) + dua);
+					ib = a*isb - b*ome*cos(the) + c*(uab(1) + dub);    
+					om = d*ome + e*(isb*cos(the) - isa*sin(the));
+					th = the + Dt*ome;	
+					//ps = th + Dt*om;	
+					/*Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));
+					Cred(0,0) = b*sin(th);
+					Cred(0,1) = b*om*cos(th);
+					Cred(1,0) = -b*cos(th);
+					Cred(1,1) = b*om*sin(th);*/
+					Ared(0,0) = d+e*Dt*(ib*sin(th)+ia*cos(th));
+					Ared(0,1) = -e*(ib*sin(th)+ia*cos(th));
+					Ared(1,0) = Dt*(d+e*Dt*(ib*sin(th)+ia*cos(th)));
+					Ared(1,1) = 1-e*Dt*(ib*sin(th)+ia*cos(th));					
+					Cred(0,0) = b*sin(th)-Dt*b*om*cos(th);
+					Cred(0,1) = b*om*cos(th);
+					Cred(1,0) = -b*cos(th)-Dt*b*om*sin(th);
+					Cred(1,1) = b*om*sin(th);		
+					Ptp = Ared*Pt1*Ared.T() + Qred;
+					Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);			
+					Pt2 = (eye(2) - Kt*Cred)*Ptp;
+					
+					
+					/*ovA(0,2) = b*sin(th);
+					ovA(0,3) = b*om*cos(th);
+					ovA(1,2) = -b*cos(th);
+					ovA(1,3) = b*om*sin(th);
+					ovA(2,0) = -e*sin(th);
+					ovA(2,1) = e*cos(th);
+					ovA(2,3) = -e*(ib*sin(th)+ia*cos(th));          	
+					ovPp = ovA*ovP*ovA.T() + ovQ;
+					tmpP = (eye(4) - ovPp*ovC.T()*inv(ovC*ovPp*ovC.T() + ovR )*ovC)*ovPp;
+					*/
+					omoo = d*om + e*(ib*cos(th) - ia*sin(th));
+					qloss(i,j) = (omoo - prww)*(omoo - prww);					
+					vomeg(i,j) = /*tmpP(2,2);/*/Pt2(0,0);
+					dloss(i,j) = qloss(i,j) + kvo*vomeg(i,j);
+					vthet(i,j) = /*tmpP(3,3);/*/Pt2(1,1);
+					vdet(i,j)  = /*det(tmpP);/*/Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0);
+					
+					if(vthet(i,j) < minloss){
+						minloss = vthet(i,j);
+						mini = i;
+						minj = j;
+					}
+				}
+			} 
+			
+			if((timeid)<1000){
+				stringstream ss;
+
+				ss << "grid/qloss" << cntr << ".txt";
+				f.open(ss.str().data(),ios::out);
+				for(i = 0; i < gsize; i++){
+					for(j = 0; j < gsize; j++){
+						f << (i - hsize)*gstep << "\t" << (j - hsize)*gstep << "\t" << qloss(i,j) << endl;
+					}
+					f << endl;
+				} 	
+				f.close();
+				
+				ss.str("");
+				ss << "grid/dloss" << cntr << ".txt";
+				f.open(ss.str().data(),ios::out);
+				for(i = 0; i < gsize; i++){
+					for(j = 0; j < gsize; j++){
+						f << (i - hsize)*gstep << "\t" << (j - hsize)*gstep << "\t" << dloss(i,j) << endl;
+					}
+					f << endl;
+				} 	
+				f.close();
+				
+				ss.str("");
+				ss << "grid/vomeg" << cntr << ".txt";
+				f.open(ss.str().data(),ios::out);
+				for(i = 0; i < gsize; i++){
+					for(j = 0; j < gsize; j++){
+						f << (i - hsize)*gstep << "\t" << (j - hsize)*gstep << "\t" << vomeg(i,j) << endl;
+					}
+					f << endl;
+				} 	
+				f.close();
+				
+				ss.str("");
+				ss << "grid/vthet" << cntr << ".txt";
+				f.open(ss.str().data(),ios::out);
+				for(i = 0; i < gsize; i++){
+					for(j = 0; j < gsize; j++){
+						f << (i - hsize)*gstep << "\t" << (j - hsize)*gstep << "\t" << vthet(i,j) << endl;
+					}
+					f << endl;
+				} 	
+				f.close();
+				
+				ss.str("");
+				ss << "grid/vdet" << cntr << ".txt";
+				f.open(ss.str().data(),ios::out);
+				for(i = 0; i < gsize; i++){
+					for(j = 0; j < gsize; j++){
+						f << (i - hsize)*gstep << "\t" << (j - hsize)*gstep << "\t" << vdet(i,j) << endl;
+					}
+					f << endl;
+				} 	
+				f.close();				
+				cntr++;
+			}
+			
+			dua = (double)(mini - hsize)*gstep;
+			dub = (double)(minj - hsize)*gstep;					
+					
+			uab(0) += dua;
+			uab(1) += dub;
+			
+			//save best variance
+			ia = a*isa + b*ome*sin(the) + c*(uab(0));
+			ib = a*isb - b*ome*cos(the) + c*(uab(1));				
+			//Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));			
+			Ared(0,0) = d+e*Dt*(ib*sin(th)+ia*cos(th));
+			Ared(0,1) = -e*(ib*sin(th)+ia*cos(th));
+			Ared(1,0) = Dt*(d+e*Dt*(ib*sin(th)+ia*cos(th)));
+			Ared(1,1) = 1-e*Dt*(ib*sin(th)+ia*cos(th));				
+			Ptp = Ared*Pt1*Ared.T() + Qred;
+			Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
+			Pt1 = (eye(2) - Kt*Cred)*Ptp;
+			/*ovA(0,2) = b*sin(th);
+			ovA(0,3) = b*om*cos(th);
+			ovA(1,2) = -b*cos(th);
+			ovA(1,3) = b*om*sin(th);
+			ovA(2,0) = -e*sin(th);
+			ovA(2,1) = e*cos(th);
+			ovA(2,3) = -e*(ib*sin(th)+ia*cos(th));          	
+			ovPp = ovA*ovP*ovA.T() + ovQ;
+			ovP = (eye(4) - ovPp*ovC.T()*inv(ovC*ovPp*ovC.T() + ovR )*ovC)*ovPp;
+		   */
+			timeid++;  
+			wwold = Ww; 
+		}
+          //*/
+		
 		//////////////////////////////////////////////////////////////////////////
 				
@@ -2028,5 +2214,5 @@
 		lq.validate();
 						
-		uab.zeros();
+		
 				
 		//create control matrix
@@ -2044,6 +2230,6 @@
 		PMSMCtrl::log_write();
 		vec tmp(2);
-		tmp(0) = double(minindex);
-		tmp(1) = double(bcbv);
+		tmp(0) = double(mini);//minindex);
+		tmp(1) = double(minj);//bcbv);
 //		tmp(1) = double(varome);
 		log_level.store( logModel , tmp); 
