Index: /library/tests/testsuite/LQG_test.cpp
===================================================================
--- /library/tests/testsuite/LQG_test.cpp (revision 1217)
+++ /library/tests/testsuite/LQG_test.cpp (revision 1222)
@@ -92,26 +92,4 @@
 }
 
-//TEST (quadratic_test){
-  /*quadraticfn qf;
-  qf.Q = chmat(2);
-  qf.Q._Ch() = mat("1 -1 0; 0 0 0; 0 0 0");
-  CHECK_CLOSE_EX(qf.eval(vec("1 2")), vec_1(1.0), 0.0000000000000001);
-  
-  LQG_universal lq;
-  lq.Losses = Array<quadraticfn>(1);
-  lq.Losses(0) = quadraticfn();
-  lq.Losses(0).Q._Ch() = mat("1 -1 0; 0 0 0; 0 0 0");
-  lq.Losses(0).rv = RV("{u up }");
-  
-  lq.Models = Array<linfnEx>(1);
-  lq.Models(0) = linfnEx(mat("1"),vec("1"));
-  lq.Models(0).rv = RV("{x }");
-  
-  lq.rv = RV("u",1);
-  
-  lq.redesign();
-  CHECK_CLOSE_EX(lq.ctrlaction(vec("1,0")), vec("1.24, -5.6"), 0.0000001);*/
-//}
-
 TEST (LQGU_static_vs_Riccati_test){
   //test of universal LQG controller
@@ -151,13 +129,5 @@
 
   //starting configuration
-  vec x0("6 3");  
- 
-	/*cout << "configuration:" << endl 
-		<< "mA:" << endl << mA << endl
-		<< "mB:" << endl << mB << endl
-		<< "mQ:" << endl << mQ << endl
-		<< "mR:" << endl << mR << endl
-		<< "mS:" << endl << mS << endl
-		<< "x0:" << endl << x0 << endl;*/
+  vec x0("6 3");  	
 
   //model
@@ -192,8 +162,5 @@
 
   lq.validate();
-  
-  //default L
-  //cout << "default L matrix:" << endl << lq.getL() << endl;
-
+    
   //produce last control matrix L
   lq.redesign();
@@ -202,8 +169,5 @@
   mat mK = mS;
   mat mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA;
-
-  //cout << "L matrix LQG_universal:" << endl << lq.getL() << endl << 
-	  //"L matrix LQG Riccati:" << endl << mL << endl;
-
+  
   //checking L matrix (universal vs Riccati), tolerance is high, but L is not main criterion
   //more important is reached loss compared in the next part
@@ -223,7 +187,4 @@
 	  mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA;
 
-	  //cout << "L matrix LQG_universal:" << endl << lq.getL() << endl << 
-		  //"L matrix LQG Riccati:" << endl << mL << endl;
-
 	  //check other times L matrix
 	  CHECK_CLOSE_EX(lq.getL().get_cols(0,1), mL, tolerr);
@@ -233,5 +194,5 @@
     
   //loss of LQG_universal
-  /*double*/vec loss_uni("0");
+  vec loss_uni("0");
 
   //setup
@@ -249,9 +210,8 @@
 	xold = x;
   }
-  /*tmploss*/ loss_uni = x.transpose() * mS * x;
-  //loss_uni += tmploss.get(0);
-
+  loss_uni = x.transpose() * mS * x;
+  
   //loss of LQG Riccati version
-  /*double*/ vec loss_rct("0");
+  vec loss_rct("0");
 
   //setup
@@ -263,12 +223,9 @@
 	u = mL * xold;
 	x = mA * xold + mB * u;
-	/*tmploss*/loss_rct = x.transpose() * mQ * x + u.transpose() * mR * u;
-	//loss_rct += tmploss.get(0);
+	loss_rct = x.transpose() * mQ * x + u.transpose() * mR * u;	
 	xold = x;
   }
-  /*tmploss*/loss_rct = x.transpose() * mS * x;
-  //loss_rct += tmploss.get(0);
-
-  //cout << "Loss LQG_universal: " << loss_uni << " vs Loss LQG Riccati: " << loss_rct << endl;
+  loss_rct = x.transpose() * mS * x;  
+  
   CHECK_CLOSE_EX(loss_uni, loss_rct, 0.0001);  
 }
@@ -307,14 +264,5 @@
   //starting configuration
   vec x0;
-	x0 = urng(matxsize);
-	
-	/*cout << "configuration:" << endl
-		<< "x size " << matxsize << "  u size " << matusize << endl
-		<< "mA:" << endl << mA << endl
-		<< "mB:" << endl << mB << endl
-		<< "mQ:" << endl << mQ << endl
-		<< "mR:" << endl << mR << endl
-		<< "mS:" << endl << mS << endl
-		<< "x0:" << endl << x0 << endl;*/
+	x0 = urng(matxsize);	
  
   vec zerovecx(matxsize);
@@ -370,5 +318,5 @@
   //checking L matrix (universal vs Riccati), tolerance is high, but L is not main criterion
   //more important is reached loss compared in the next part
-  double tolerr = 2;//1;//0.01; //0.01 OK x 0.001 NO OK
+  double tolerr = 2;
 
   //check last time L matrix
@@ -491,6 +439,5 @@
 	int n, k;
 	double Br;
-	//double x00;
-
+	
 	for(k = K-1; k >= 0;k--){
 			lq.redesign();
@@ -499,7 +446,6 @@
 
 for(P = 0.01; P <= 10; (P*=10)){
-	lq.resetTime();
-	
-//cout << "Ls " << L << endl;
+	lq.resetTime();	
+
 	for(n = 0; n < N; n++){        
         Br = Eb + sqrt(P)*randn();        
@@ -516,16 +462,4 @@
 	}
     
-	/*vec mtraj(K);
-	for(k=0;k<K;k++){
-		mtraj(k) = 0;
-		for(n = 0; n < N; n++) mtraj(k) += xn(k, n);
-		mtraj(k) /= N;
-		mtraj(k) += yr;
-	}
-	cout << "prum trajek " << mtraj << endl;*/
-    //cout << "prum ztrata " << sum(loss)/N << endl;
-    //rfloss = mean(loss);
-    //rftraj = (mean((xn(1:K, :)), 2) + yr*ones(K, 1))';
-
 	double tolerr;
 	//double P = 0.01;	//loss ~ 1.0???		e-2
@@ -559,6 +493,5 @@
 		x0(0) = y0 - yr;
 		x0(1) = Eb;
-		//x0(2) = inP;
-	       
+			       
     mat A("1 0 0; 0 1 0; 0 0 1"); 
     mat B("0; 0; 0");
@@ -568,8 +501,5 @@
 		Q.zeros();
 		Q(1,1) = sigma*sigma;
-    //mat R("0");   
-	//mat P(3,3);
-	//	P.zeros();
-    
+        
     vec u(K);
 		u.zeros();
@@ -582,8 +512,5 @@
 	mat xn2(K+1, N);
 		xn2.zeros();
-    //vec S(K);            
-	//	S.zeros();
-    //vec L(K);        
-    //    L.zeros();
+    
 	mat L(1, 3);
 		L.zeros();
@@ -597,35 +524,28 @@
 
 	Array<linfnEx> model(2);  
-  model(0).A = A;
-  model(0).B = vec("0 0");
-  model(0).rv = RV("x", 3, 0);
-  model(0).rv_ret = RV("x", 3, 1);  
-  model(1).A = B;
-  model(1).B = vec("0 0");
-  model(1).rv = RV("u", 1, 0);
-  model(1).rv_ret = RV("x", 3, 1);  
-  lq.Models = model;
-
-  Array<quadraticfn> qloss(2);  
-  qloss(0).Q.setCh(X);
-  qloss(0).rv = RV("x", 3, 1);  
-  qloss(1).Q.setCh(Y);
-  qloss(1).rv = RV("u", 1, 0);  
-  lq.Losses = qloss;
+     model(0).A = A;
+     model(0).B = vec("0 0");
+     model(0).rv = RV("x", 3, 0);
+     model(0).rv_ret = RV("x", 3, 1);  
+     model(1).A = B;
+     model(1).B = vec("0 0");
+     model(1).rv = RV("u", 1, 0);
+     model(1).rv_ret = RV("x", 3, 1);  
+     lq.Models = model;
+
+     Array<quadraticfn> qloss(2);  
+     qloss(0).Q.setCh(X);
+     qloss(0).rv = RV("x", 3, 1);  
+     qloss(1).Q.setCh(Y);
+     qloss(1).rv = RV("u", 1, 0);  
+     lq.Losses = qloss;
   
-  lq.finalLoss.Q.setCh(X);
-  lq.finalLoss.rv = RV("x", 3, 1);
-
-  lq.validate();  
+     lq.finalLoss.Q.setCh(X);
+     lq.finalLoss.rv = RV("x", 3, 1);
+
+     lq.validate();  
           
 	int n, k;
-	//double Br;
-	//double x00;
-
-	/*for(k = K-1; k >= 0;k--){
-		lq.redesign();
-		L(k) = (lq.getL())(0,0);
-	}*/
-//cout << "Ls " << L << endl;
+	
 for(inP = 0.01; inP <= 10; (inP*=10)){
 	lq.resetTime();
@@ -668,32 +588,17 @@
 				   u(k) = L(0)*xn0(k, n) + L(1)*xn1(k, n) + L(2)*xn2(k, n);
 				   Kk(k) = u(k)*xn2(k, n)/(u(k)*u(k)*xn2(k, n) + sigma*sigma);
-                   xn0(k+1, n) = xn0(k, n) + xn1(k, n)*u(k) + sigma*randn();
-                   xn1(k+1, n) = xn1(k, n) + Kk(k)*(xn0(k+1, n) - xn0(k, n) - xn1(k, n)*u(k));
-                   xn2(k+1, n) = (1 - Kk(k)*u(k))*xn2(k, n);                                       
+                   	   xn0(k+1, n) = xn0(k, n) + xn1(k, n)*u(k) + sigma*randn();
+                       xn1(k+1, n) = xn1(k, n) + Kk(k)*(xn0(k+1, n) - xn0(k, n) - xn1(k, n)*u(k));
+                       xn2(k+1, n) = (1 - Kk(k)*u(k))*xn2(k, n);                                       
 				}
 
 
-                loss(n) = 0;
+                    loss(n) = 0;
 				for(k=0;k<K;k++) loss(n) += xn0(k, n)*xn0(k, n);
                 
-	}
-    
-	/*vec mtraj(K);
-	for(k=0;k<K;k++){
-		mtraj(k) = 0;
-		for(n = 0; n < N; n++) mtraj(k) += xn0(k, n);
-		mtraj(k) /= N;
-		mtraj(k) += yr;
-	}
-	cout << "prum trajek " << mtraj << endl;*/
-    //cout << "prum ztrata " << sum(loss)/N << endl;
+	}	
     
 	double tolerr = 0.2;	
-	/*if(inP == 0.01) tolerr = 0.2;
-	else if(inP == 0.1) tolerr = 2;
-	else if(inP == 1) tolerr = 2000;
-	else if(inP == 10) tolerr = 2e+7;
-	else tolerr = 0;*/
-
+	
 	CHECK_CLOSE_EX(1.0, sum(loss)/N, tolerr);
 	}
@@ -837,8 +742,8 @@
 				else if(u(1) < -50) u(1) = -50;
             
-				x(0, k+1) = a*x(0, k) + b*(x(2, k) + x(4, k))*sin(x(3, k)) + c*u(0);// + sqrt(Q(0, 0))*randn();  
-				x(1, k+1) = a*x(1, k) - b*(x(2, k) + x(4, k))*cos(x(3, k)) + c*u(1);// + sqrt(Q(1, 1))*randn();
-				x(2, k+1) = d*x(2, k) + (d-1)*x(4, k) + e*(x(1, k)*cos(x(3, k)) - x(0, k)*sin(x(3, k)));// + sqrt(Q(2, 2))*randn();
-				x(3, k+1) = x(3, k) + (x(2, k) + x(4, k))*DELTAt;// + sqrt(Q(3, 3))*randn(); 
+				x(0, k+1) = a*x(0, k) + b*(x(2, k) + x(4, k))*sin(x(3, k)) + c*u(0);  
+				x(1, k+1) = a*x(1, k) - b*(x(2, k) + x(4, k))*cos(x(3, k)) + c*u(1);
+				x(2, k+1) = d*x(2, k) + (d-1)*x(4, k) + e*(x(1, k)*cos(x(3, k)) - x(0, k)*sin(x(3, k)));
+				x(3, k+1) = x(3, k) + (x(2, k) + x(4, k))*DELTAt;
 				x(4, k+1) = x(4, k); 				
 			}
@@ -883,5 +788,5 @@
 			x(4, k+1) = x(4, k); 
 	
-			losses(n) += (x.get_col(k+1).transpose() * X * x.get_col(k+1))(0);// + (u.transpose() * Y * u)(0);  			
+			losses(n) += (x.get_col(k+1).transpose() * X * x.get_col(k+1))(0); 			
 		}
 
@@ -889,13 +794,5 @@
 	}
 
-	cout << "Ztrata: " << sum(losses)/N << endl;
-	//cout << "Trajektorie:\n" << omega(0,Kt-1)/N << endl;
-	
-	/*ofstream log;
-	log.open ("log.txt");
-	for(k = 0; k < Kt; k++)
-		log << k << "\t" << omega(k)/N + OMEGAt << endl;
-	log.close();*/
-
+	cout << "Loss: " << sum(losses)/N << endl;
 }
 
@@ -944,6 +841,5 @@
 	qloss(0).Q.setCh(mat("1"));
 	qloss(0).rv = RV("yt", 1, 0);	
-	qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2)
-	//qloss(1).Q = mat("0.001");//automatic sqrt
+	qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2)	
 	qloss(1).rv = RV("ut", 1, 0); 	
 	lq.Losses = qloss;
@@ -957,10 +853,10 @@
 	lq.redesign();
 
-	for(int k = K-1; k > 0; k--){
-		//cout << "L: " << lq.getL() << endl;
+	for(int k = K-1; k > 0; k--){		
 		lq.redesign();
 	}
 
-	cout << "L: " << lq.getL() << endl;
+	mat resref("-69.4086   47.2578   -0.2701   0.0");
+	CHECK_CLOSE_EX(resref, lq.getL(), 0.1);
 }
 
@@ -976,6 +872,5 @@
 
 	int K = 150;
-	//yr = 10.0;
-
+	
 	LQG_universal lq;
 	lq.rv = RV("ut", 1, 0); 
@@ -1023,10 +918,10 @@
 	lq.redesign();
 
-	for(int k = K-1; k > 0; k--){
-		//cout << "L: " << lq.getL() << endl;
+	for(int k = K-1; k > 0; k--){		
 		lq.redesign();
 	}
 
-	cout << "L: " << lq.getL() << endl;
+	mat resref("-69.4086   47.2578   -0.2701   233.758");
+	CHECK_CLOSE_EX(resref, lq.getL(), 0.1);
 }
 
@@ -1041,7 +936,5 @@
 	*/
 
-	int K = 200;
-
-	//double yr = 10.0;
+	int K = 200;	
 
 	LQG_universal lq;
@@ -1083,6 +976,5 @@
 	qloss(0).rv = RV("yt", 1, 0);
     	qloss(0).rv.add(RV("yr", 1, 0));
-	qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2)
-	//qloss(1).Q = mat("0.001");//automatic sqrt
+	qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2)	
 	qloss(1).rv = RV("ut", 1, 0); 	
 	lq.Losses = qloss;
@@ -1097,10 +989,10 @@
 	lq.redesign();
 
-	for(int k = K-1; k > 0; k--){
-		//cout << "L: " << lq.getL() << endl;
+	for(int k = K-1; k > 0; k--){		
 		lq.redesign();
 	}
 
-	cout << "L: " << lq.getL() << endl;
+	mat resref("-69.4086   47.2578   -0.2701   23.3758   0.0");
+	CHECK_CLOSE_EX(resref, lq.getL(), 0.1);
 }
 
@@ -1123,7 +1015,5 @@
 
 	int K = 50;
-	double w = 5.0;
-	double q0 = 0.1;
-
+	
 	LQG_universal lq;
 	lq.rv = RV("u", 1, 0); 
@@ -1191,397 +1081,18 @@
 	for(k = K-1; k > 0; k--) lq.redesign();
 	
-	cout << "L: " << lq.getL() << endl;
-
-	vec q(50);
-	q(0) = q0;
-
-	vec u(50);
-
-	//rvc = [q(k), q(k-1), q(k-2), u(k-1), u(k-2), w(k)]
-	vec xrvc(6);
-	xrvc(5) = w;
+	mat resref("-12.1148   7.9949   -1.6191   -0.6123   0.1836   7.1642 0.0");
+	CHECK_CLOSE_EX(resref, lq.getL(), 0.01);	
+}
+
+TEST (Matrix_Division){	
+	Uniform_RNG urng;
+
+	mat A("1 1 1 1 1; 0 2 2 2 2; 0 0 3 3 3; 0 0 0 4 4; 0 0 0 0 5");
+	mat B = urng(5,5);		
+	mat C1, C2;
+
+	C1 = inv(A)*B;	
+	ldutg(A,B,C2);	
 		
-
-	//first step
-	xrvc(0) = q(0);
-	xrvc(1) = q0;
-	xrvc(2) = q0;
-	xrvc(3) = 0;
-	xrvc(4) = 0;
-	u(0) = (lq.ctrlaction(xrvc))(0);
-	q(1) = 1.654 * q(0) - 1.022*q0 + 0.2019*q0 + 0.1098*u(0);
-
-	//second step
-	xrvc(0) = q(1);
-	xrvc(1) = q(0);
-	xrvc(2) = q0;
-	xrvc(3) = u(0);
-	xrvc(4) = 0;
-	u(1) = (lq.ctrlaction(xrvc))(0);
-	q(2) = 1.654 * q(1) - 1.022*q(0) + 0.2019*q0 + 0.1098*u(1) + 0.0792*u(0);
-
-	//cycle
-	for(k = 2; k < K-1; k++){
-		xrvc(0) = q(k);
-		xrvc(1) = q(k-1);
-		xrvc(2) = q(k-2);
-		xrvc(3) = u(k-1);
-		xrvc(4) = u(k-2);
-		u(k) = (lq.ctrlaction(xrvc))(0);
-		q(k+1) = 1.654 * q(k) - 1.022*q(k-1) + 0.2019*q(k-2) + 0.1098*u(k) + 0.0792*u(k-1) - 0.0229*u(k-2);
-	}
-	
-	
-	ofstream log;
-	log.open ("log.txt");
-	for(k = 0; k < K; k++)
-		log << k << "\t" << q(k) << endl;
-	log.close();
-}
-
-/*TEST (matrixdivisionspeedtest){
-	Real_Timer tmr;
-	Uniform_RNG urng;
-
-	mat A("0.0319219"); //1 1 1 1; 0 2 2 2 2; 0 0 3 3 3; 0 0 0 4 4; 0 0 0 0 5");
-	mat B("0.24835 -0.112361 0.000642142 -1.3721");// = urng(2,6);		
-	mat C(1,4);
-		C.zeros();
-
-	int n = 1;
-	int j,k,l,m;
-	
-	//cout << "A:" << endl << A << endl << "B:" << endl << B << endl;
-
-	cout << "Using inv function: ";
-	tmr.tic();
-		for(j = 0; j < n; j++) C = inv(A)*B;	
-	tmr.toc_print();
-	cout << endl << C << endl;
-	
-	cout << "Using ldutg function: ";
-	tmr.tic();
-		for(j = 0; j < n; j++) ldutg(A,B,C);	
-	tmr.toc_print();
-	cout << endl << C << endl;*/
-
-	/*cout << "Using Gauss elimination: ";
-		int utrows = A.rows();			   
-		int longrow = utrows + B.cols();		    
-		mat Cc(utrows,longrow);
-			Cc.zeros();
-	tmr.tic();
-		for(j = 0; j < n; j++){  
-		    Cc.set_submatrix(0,utrows-1,0,utrows-1,A);//rrccM
-		    Cc.set_submatrix(0,utrows-1,utrows,longrow-1,B);		    
-		    for(k = utrows-1; k >= 0; k--){        
-			   Cc.set_row(k, Cc.get_row(k) / A(k,k));        
-			   for(l = 0;l < k; l++){            
-				  Cc.set_row(l,Cc.get_row(l) - A(l,k)*Cc.get_row(k));
-			   } 			   			          
-		    }    
-		    C = Cc(0,utrows-1,utrows,longrow-1);
-		}
-	tmr.toc_print();
-	cout << endl << C << endl;*/
-	
-	/*cout << "Using Gauss elimination Direct access!: ";
-	 //data da pointer co jede jako pole po sloupcich 
-	tmr.tic();
-		for(j = 0; j < n; j++){  
-		    
-		    		for(l=0;l<25;l++)
-		    			(Cc._data())[l] = (A._data())[l];
-		    		for(l=25;l<50;l++)
-		    			Cc._data()[l] = B._data()[l-25];		    
-		   
-		    		    
-		    for(k = utrows-1; k >= 0; k--){       
-		    	   for(l=0;l<10;l++) Cc._data()[k+5*l] /= A._data()[k+5*k];	 			           
-			   for(l = 0;l < k; l++){
-			   	  for(m=0;m<10;m++)	Cc._data()[l+5*m] -= A._data()[l+5*k]*Cc._data()[k+5*m];            				  
-			   } 			   			          
-		    }    
-		    
-		    for(k=0;k<25;k++)
-		    		
-		    			C._data()[k] = Cc._data()[k+25];
-		}
-		
-	tmr.toc_print();		
-	cout << endl << C << endl;
-		
-	cout << "Using Gauss elimination ARRAY: ";
-	tmr.tic();
-	double arA[5][5];		    
-	double arB[5][5];		    
-	double arC[5][5];		    
-	double arCc[5][10];
-	for(k=0;k<5;k++){
-		for(l=0;l<5;l++){
-			arA[k][l] = A(k,l);
-			arB[k][l] = B(k,l);
-		}
-	}
-					    
-			
-	
-		for(j = 0; j < n; j++){  
-		    for(k=0;k<5;k++){
-		    		for(l=0;l<5;l++)
-		    			arCc[k][l] = arA[k][l];
-		    		for(l=5;l<10;l++)
-		    			arCc[k][l] = arB[k][l-5];		    
-		    }
-		    		    
-		    for(k = utrows-1; k >= 0; k--){       
-		    	   for(l=0;l<10;l++) arCc[k][l] /= arA[k][k];	 			           
-			   for(l = 0;l < k; l++){
-			   	  for(m=0;m<10;m++)	arCc[l][m] -= arA[l][k]*arCc[k][m];            				  
-			   } 			   			          
-		    }    
-		    
-		    for(k=0;k<5;k++)
-		    		for(l=0;l<5;l++)
-		    			arC[k][l] = arCc[k][l+5];
-		}
-		
-		
-	
-	for(k=0;k<5;k++)
-		for(l=0;l<5;l++)
-		    	C(k,l) = arC[k][l];			
-	tmr.toc_print();
-	cout << endl << C << endl;*/
-//}
-
-//TEST (LQGU_large_test){
-	/*
-	loss = /Q1*x(+1) + /Q2*y + /Q3*z + /Q4*u + /Q5*[x(+1); y; 1] + /Q6*[z; u; 1] + /Q7*[x(+1); y; z; u; 1]
-	u = rv
-	x = rvc
-	y = A1*[x; 1] + B1
-	z = A2*[x; u; 1] + B2
-	x(+1) = A3*[x; 1] + B3
-	*/
-	//uniform random generator
-  /*Uniform_RNG urng;  
-  int max_size = 10;
-  double maxmult = 10.0;
-  
-  urng.setup(2.0, max_size);  
-  int xsize = round(urng());    
-  int usize = round(urng());
-  int ysize = round(urng());
-  int zsize = round(urng());
-  cout << "CFG: " << xsize << " " << ysize << " " << zsize << " " << usize << " " << endl;
-  urng.setup(-maxmult, maxmult);
-  mat tmpmat;
-  mat A1,A2,A3,Q1,Q2,Q3,Q4,Q5,Q6,Q7;
-  vec B1,B2,B3;
-
-  A1 = urng(ysize,xsize+1);
-  B1 = urng(ysize);
-  A2 = urng(zsize,xsize+usize+1);
-  B2 = urng(zsize);
-  A3 = urng(xsize,xsize+1);
-  B3 = urng(xsize);
-
-  tmpmat = urng(xsize,xsize);
-  Q1 = tmpmat*tmpmat.transpose();
-  tmpmat = urng(ysize,ysize);
-  Q2 = tmpmat*tmpmat.transpose();
-  tmpmat = urng(zsize,zsize);
-  Q3 = tmpmat*tmpmat.transpose();
-  tmpmat = urng(usize,usize);
-  Q4 = tmpmat*tmpmat.transpose();
-  tmpmat = urng(xsize+ysize+1,xsize+ysize+1);
-  Q5 = tmpmat*tmpmat.transpose();
-  tmpmat = urng(zsize+usize+1,zsize+usize+1);
-  Q6 = tmpmat*tmpmat.transpose();
-  tmpmat = urng(xsize+ysize+zsize+usize+1,xsize+ysize+zsize+usize+1);
-  Q7 = tmpmat*tmpmat.transpose();
-
-    
-  //starting configuration
-  vec x0;
-	x0 = urng(xsize);  
-
-//test of universal LQG controller
-  LQG_universal lq;
-  lq.rv = RV("u", usize, 0);  
-  lq.set_rvc(RV("x", xsize, 0));
-  lq.horizon = 100;
-
-	RV tmprv;
-
-  //model
-  Array<linfnEx> model(3);  
-  model(0).A = A1;
-  model(0).B = B1;
-  tmprv = RV("x", xsize, 0);
-  tmprv.add(RV("1",1,0));
-  model(0).rv = tmprv;
-  model(0).rv_ret = RV("y", ysize, 0);    
-  model(1).A = A2;
-  model(1).B = B2;
-  tmprv = RV("x", xsize, 0);
-  tmprv.add(RV("u", usize, 0));
-  tmprv.add(RV("1",1,0));
-  model(1).rv = tmprv;
-  model(1).rv_ret = RV("z", zsize, 0);
-  model(2).A = A3;
-  model(2).B = B3;
-  tmprv = RV("x", xsize, 0);
-  tmprv.add(RV("1",1,0));
-  model(2).rv = tmprv;
-  model(2).rv_ret = RV("x", xsize, 1);
-  //setup
-  lq.Models = model;
-
-  //loss
-  Array<quadraticfn> loss(7);  
-  loss(0).Q.setCh(Q1);
-  loss(0).rv = RV("x", xsize, 1);
-  loss(1).Q.setCh(Q2);
-  loss(1).rv = RV("y", ysize, 0);
-  loss(2).Q.setCh(Q3);
-  loss(2).rv = RV("z", zsize, 0);
-  loss(3).Q.setCh(Q4);
-  loss(3).rv = RV("u", usize, 0);
-  loss(4).Q.setCh(Q5);
-  tmprv = RV("x", xsize, 1);
-  tmprv.add(RV("y", ysize, 0));
-  tmprv.add(RV("1",1,0));
-  loss(4).rv = tmprv;
-  loss(5).Q.setCh(Q6);
-  tmprv = RV("z", zsize, 0);
-  tmprv.add(RV("u", usize, 0));
-  tmprv.add(RV("1",1,0));
-  loss(5).rv = tmprv;
-  loss(6).Q.setCh(Q7);
-  tmprv = RV("x", xsize, 1);
-  tmprv.add(RV("y", ysize, 0));
-  tmprv.add(RV("z", zsize, 0));
-  tmprv.add(RV("u", usize, 0));
-  tmprv.add(RV("1",1,0));
-  loss(6).rv = tmprv;
-  //setup
-  lq.Losses = loss;
-
-  //finalloss setup
-  tmpmat = urng(xsize,xsize);	
-  lq.finalLoss.Q.setCh(tmpmat*tmpmat.transpose());
-  lq.finalLoss.rv = RV("x", xsize, 1);
-
-  lq.validate();  
-
-  //produce last control matrix L
-  lq.redesign();
-  cout << lq.getL() << endl;  
-  
-  int i;  
-  for(i = 0; i < lq.horizon - 1; i++) {
-	  lq.redesign();	  
-  }
-  cout << lq.getL() << endl;
-
-  mat K;
-  K = A3.get_cols(0, xsize-1).transpose()*Q1.transpose()*Q1*A3.get_cols(0, xsize-1)
-    + A1.get_cols(0, xsize-1).transpose()*Q2.transpose()*Q2*A1.get_cols(0, xsize-1)
-	+ A2.get_cols(0, xsize-1).transpose()*Q3.transpose()*Q3*A2.get_cols(0, xsize-1)
-	+ A3.get_cols(0, xsize-1).transpose()*Q5.get_cols(0, xsize-1).transpose()*Q5.get_cols(0, xsize-1)*A3.get_cols(0, xsize-1)
-	+ A1.get_cols(0, xsize-1).transpose()*Q5.get_cols(xsize, xsize+ysize-1).transpose()*Q5.get_cols(xsize, xsize+ysize-1)*A1.get_cols(0, xsize-1) 
-	+ A2.get_cols(0, xsize-1).transpose()*Q6.get_cols(0, zsize-1).transpose()*Q6.get_cols(0, zsize-1)*A2.get_cols(0, xsize-1)
-	+ A3.get_cols(0, xsize-1).transpose()*Q7.get_cols(0, xsize-1).transpose()*Q7.get_cols(0, xsize-1)*A3.get_cols(0, xsize-1)
-	+ A1.get_cols(0, xsize-1).transpose()*Q7.get_cols(xsize, xsize+ysize-1).transpose()*Q7.get_cols(xsize, xsize+ysize-1)*A1.get_cols(0, xsize-1)
-	+ A2.get_cols(0, xsize-1).transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1).transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1)*A2.get_cols(0, xsize-1);
-
-  mat L;
-  L = A2.get_cols(xsize, xsize+usize-1).transpose()*Q3.transpose()*Q3*A2.get_cols(xsize, xsize+usize-1)
-    + Q4.transpose()*Q4
-	+ A2.get_cols(xsize, xsize+usize-1).transpose()*Q6.get_cols(0, zsize-1).transpose()*Q6.get_cols(0, zsize-1)*A2.get_cols(xsize, xsize+usize-1)
-	+ Q6.get_cols(zsize, zsize+usize-1).transpose()*Q6.get_cols(zsize, zsize+usize-1)
-	+ A2.get_cols(xsize, xsize+usize-1).transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1).transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1)*A2.get_cols(xsize, xsize+usize-1)
-	+ Q7.get_cols(xsize+ysize+zsize, xsize+ysize+zsize+usize-1).transpose()*Q7.get_cols(xsize+ysize+zsize, xsize+ysize+zsize+usize-1);
-
-  double M;
-  M = (A3.get_cols(xsize,xsize).transpose()*Q1.transpose()*Q1*A3.get_cols(xsize,xsize))(0,0)
-    + (B3.transpose()*Q1.transpose()*Q1*B3)(0)
-	+ (A1.get_cols(xsize,xsize).transpose()*Q2.transpose()*Q2*A1.get_cols(xsize,xsize))(0,0)
-	+ (B1.transpose()*Q2.transpose()*Q2*B1)(0)
-	+ (A2.get_cols(xsize+usize,xsize+usize).transpose()*Q3.transpose()*Q3*A2.get_cols(xsize+usize,xsize+usize))(0,0)
-	+ (B2.transpose()*Q3.transpose()*Q3*B2)(0)
-	+ (A3.get_cols(xsize,xsize).transpose()*Q5.get_cols(0, xsize-1).transpose()*Q5.get_cols(0, xsize-1)*A3.get_cols(xsize,xsize))(0,0)
-	+ (B3.transpose()*Q5.get_cols(0, xsize-1).transpose()*Q5.get_cols(0, xsize-1)*B3)(0)
-	+ (A1.get_cols(xsize,xsize).transpose()*Q5.get_cols(xsize, xsize+ysize-1).transpose()*Q5.get_cols(xsize, xsize+ysize-1)*A1.get_cols(xsize,xsize))(0,0)
-	+ (B1.transpose()*Q5.get_cols(xsize, xsize+ysize-1).transpose()*Q5.get_cols(xsize, xsize+ysize-1)*B1)(0)
-	+ (Q5.get_cols(xsize+ysize,xsize+ysize).transpose()*Q5.get_cols(xsize+ysize,xsize+ysize))(0,0)
-	+ (A2.get_cols(xsize+usize,xsize+usize).transpose()*Q6.get_cols(0, zsize-1).transpose()*Q6.get_cols(0, zsize-1)*A2.get_cols(xsize+usize,xsize+usize))(0,0)
-	+ (B2.transpose()*Q6.get_cols(0, zsize-1).transpose()*Q6.get_cols(0, zsize-1)*B2)(0)
-	+ (Q6.get_cols(zsize+usize,zsize+usize).transpose()*Q6.get_cols(zsize+usize,zsize+usize))(0,0)
-	+ (A3.get_cols(xsize,xsize).transpose()*Q7.get_cols(0, xsize-1).transpose()*Q7.get_cols(0, xsize-1)*A3.get_cols(xsize,xsize))(0,0)
-	+ (B3.transpose()*Q7.get_cols(0, xsize-1).transpose()*Q7.get_cols(0, xsize-1)*B3)(0)
-	+ (A1.get_cols(xsize,xsize).transpose()*Q7.get_cols(xsize, xsize+ysize-1).transpose()*Q7.get_cols(xsize, xsize+ysize-1)*A1.get_cols(xsize,xsize))(0,0)
-	+ (B1.transpose()*Q7.get_cols(xsize, xsize+ysize-1).transpose()*Q7.get_cols(xsize, xsize+ysize-1)*B1)(0)
-	+ (A2.get_cols(xsize+usize,xsize+usize).transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1).transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1)*A2.get_cols(xsize+usize,xsize+usize))(0,0)
-	+ (B2.transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1).transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1)*B2)(0)
-	+ (Q7.get_cols(xsize+ysize+zsize+usize,xsize+ysize+zsize+usize).transpose()*Q7.get_cols(xsize+ysize+zsize+usize,xsize+ysize+zsize+usize))(0,0);*/
-
-  
-  /*mat res;
-  res = -inv(sqrtm(L))*sqrtm(K);*/
-
-  /*cout << -inv(sqrtm(L))*sqrtm(K).get_cols(0,L.rows()).get_rows(0,L.cols()) << endl;
-}*/
-
-/*TEST (validation_test){
-	RV rv1("x", 1);
-	RV rv2("y", 2);
-	RV rv3("z", 3);
-	RV rv4("u", 2);
-	RV rv5("v", 1);
-	RV all;
-	all = rv1;
-	all.add(rv2);
-	all.add(rv3);
-	all.add(rv4);
-	all.add(rv5);
-	all.add(RV("1",1,0));
-	//cout << "all rv: " << all << endl;
-
-	ivec fy = rv2.findself(all);
-	//cout << "finding y: " << fy << endl;
-	ivec dy = rv2.dataind(all);
-	//cout << "dataind y: " << dy << endl;
-
-	RV rvzyu;
-	rvzyu = rv3;
-	rvzyu.add(rv2);
-	rvzyu.add(rv4);
-	fy = rvzyu.findself(all);
-	//cout << "finding zyu: " << fy << endl;
-	dy = rvzyu.dataind(all);
-	//cout << "dataind zyu: " << dy << endl;
-
-	rvzyu.add(RV("1",1,0));
-	fy = rvzyu.findself(all);
-	//cout << "finding zyu1: " << fy << endl;
-	dy = rvzyu.dataind(all);
-	//cout << "dataind zyu1: " << dy << endl;
-
-	rvzyu.add(RV("k",1,0));
-	fy = rvzyu.findself(all);
-	//cout << "finding zyu1 !k!: " << fy << endl;
-	dy = rvzyu.dataind(all);
-	//cout << "dataind zyu1 !k!: " << dy << endl;
-
-	RV emptyrv;
-	fy = emptyrv.findself(all);
-	//cout << "finding empty: " << fy << " size " << fy.size() << endl;
-	dy = emptyrv.dataind(all);
-	//cout << "dataind empty: " << dy << " size " << dy.size() << endl;
-
-	LQG_universal lq;
-	lq.validate();
-}*/
+	CHECK_CLOSE_EX(C1, C2, 0.000001);	
+}
