Changeset 1205

Timestamp:

09/29/10 18:19:04 (14 years ago)

Author:

vahalam

Message:

uprava testu

Files:

• library/tests/testsuite/LQG_test.cpp (modified) (9 diffs)

library/tests/testsuite/LQG_test.cpp

@@ -92 +92 @@
 }
 
-TEST (quadratic_test){
+//TEST (quadratic_test){
   /*quadraticfn qf;
   qf.Q = chmat(2);
@@ -112 +112 @@
   lq.redesign();
   CHECK_CLOSE_EX(lq.ctrlaction(vec("1,0")), vec("1.24, -5.6"), 0.0000001);*/
-}
+//}
 
 TEST (LQGU_static_vs_Riccati_test){
@@ -180 +180 @@
   //loss x'Qx part
   loss(0).Q.setCh(mQ);
-  loss(0).rv = RV("x", 2, 0);
+  loss(0).rv = RV("x", 2, 1);
   //loss u'Ru part
   loss(1).Q.setCh(mR);
@@ -348 +348 @@
   //loss x'Qx part
   loss(0).Q.setCh(mQ);
-  loss(0).rv = RV("x", matxsize, 0);
+  loss(0).rv = RV("x", matxsize, 1);
   //loss u'Ru part
   loss(1).Q.setCh(mR);
@@ -479 +479 @@
   Array<quadraticfn> qloss(2);  
   qloss(0).Q.setCh(Q);
-  qloss(0).rv = RV("x", 1, 0);  
+  qloss(0).rv = RV("x", 1, 1);  
   qloss(1).Q.setCh(R);
   qloss(1).rv = RV("u", 1, 0);  
@@ -609 +609 @@
   Array<quadraticfn> qloss(2);  
   qloss(0).Q.setCh(X);
-  qloss(0).rv = RV("x", 3, 0);  
+  qloss(0).rv = RV("x", 3, 1);  
   qloss(1).Q.setCh(Y);
   qloss(1).rv = RV("u", 1, 0);  
@@ -700 +700 @@
 }
 
-TEST (LQGU_PMSM_test){
-        bdm_assert(0, "Test not implemented.");
+/*TEST (LQGU_PMSM_test){
+        
+        int K = 10;
+        int Kt = 100;
+        int N = 50;
+
+        double a = 0.9898;
+        double b = 0.0072;
+        double c = 0.0361;
+        double d = 1;
+        double e = 0.0149; 
+
+        double OMEGAt = 2.15;
+        double DELTAt = 0.000125;
+
+        double v = 0.0001;
+        double w = 1;
+
+        mat A(5,5);
+        A.zeros();
+        A(0,0) = a;
+        A(1,1) = a;
+        A(2,2) = d;
+        A(3,3) = 1.0;
+        A(4,4) = 1.0;
+        A(2,4) = d-1.0;
+        A(3,2) = DELTAt;
+        A(3,4) = DELTAt;
+
+        mat B(5,2);
+        B.zeros();
+        B(0,0) = c;
+        B(1,1) = c;
+
+        mat C(2,5);
+        C.zeros();
+        C(0,0) = c;
+        C(1,1) = c;
+         
+        mat X(5,5);
+        X.zeros();
+        X(2,2) = w;
+
+        mat Y(2,2);
+        Y.zeros();
+        Y(0,0) = v;
+        Y(1,1) = v;
+
+        mat Q(5,5);
+        Q.zeros();
+        Q(0,0) = 0.0013;
+        Q(1,1) = 0.0013;
+        Q(2,2) = 5e-6;
+        Q(3,3) = 1e-10;
+
+        mat R(2,2);
+        R.zeros();
+        R(0,0) = 0.0006;
+        R(0,0) = 0.0006;
+
+        vec x0(5);
+        x0.zeros();
+        x0(2) = 1.0-OMEGAt;
+        x0(3) = itpp::pi / 2;
+        x0(4) = OMEGAt;
+
+        mat P(5,5);
+        P.zeros();
+        P(0,0) = 0.01;
+        P(1,1) = 0.01;
+        P(2,2) = 0.01;
+        P(3,3) = 0.01;
+
+
+        //vec u0(Kt+K);
+        //vec u1(Kt+K);
+        //mat u(2,Kt+K)
+        vec u(2);
+
+        //vec x0(Kt+K);
+        //vec x1(Kt+K);
+        //vec x2(Kt+K);
+        //vec x3(Kt+K);
+        //vec x4(Kt+K);
+        mat x(5,Kt+K);
+
+        //vec y0(Kt+K);
+        //vec y1(Kt+K);
+        mat y(2, Kt+K);
+
+        mat Pk(5, 5);
+        
+        mat KK(5, 2);
+
+        mat L(2, 5);
+        mat L0(5, Kt);
+        mat L1(5, Kt); 
+
+        int i,n,k,kt;
+        vec x00(5);
+        vec randvec(2);
+
+        LQG_universal lq;
+        lq.rv = RV("u", 2, 0);  
+        lq.set_rvc(RV("x", 5, 0));
+        lq.horizon = K;
+
+        Array<linfnEx> model(2);  
+        model(0).A = A;
+        model(0).B = vec("0 0 0 0 0");
+        model(0).rv = RV("x", 5, 0);
+        model(0).rv_ret = RV("x", 5, 1);  
+        model(1).A = B;
+        model(1).B = vec("0 0");
+        model(1).rv = RV("u", 2, 0);
+        model(1).rv_ret = RV("x", 5, 1);  
+        lq.Models = model;
+
+        Array<quadraticfn> qloss(2);  
+        qloss(0).Q.setCh(X);
+        qloss(0).rv = RV("x", 5, 1);  
+        qloss(1).Q.setCh(Y);
+        qloss(1).rv = RV("u", 2, 0);  
+        lq.Losses = qloss;
+  
+        lq.finalLoss.Q.setCh(X);
+        lq.finalLoss.rv = RV("x", 5, 1);
+
+        lq.validate(); 
+
+        vec losses(N);
+                losses.zeros();
+
+        double loss;
+        
+        for(n = 0; n < N; n++) {
+                L0.zeros();
+                L1.zeros();
+                Pk.zeros();
+                                    
+                for(i=0;i<4;i++) x00(i) = x0(i) + sqrt(P(i,i))*randn();   
+                       
+                for(kt = 0; kt < Kt; kt++){            
+                        x.set_col(0, x00);
+                        y.set_col(0, x.get_col(0).get(0,1));            
+                        for(k = 0; k < kt+K-1; k++){
+                                L.set_size(2,5);
+                                L.set_row(0, L0.get_col(kt));
+                                L.set_row(1, L1.get_col(kt));
+                                u = L*x.get_col(k);
+
+                                /*if(k > 2){
+                                                cout << "Pk " << Pk << endl;
+                                                cout << "C " << C << endl;
+                                                cout << "R " << R << endl;
+                                                cout << "Pk * C.transpose() " << Pk * C.transpose() << endl;
+                                                cout << "inv(C * Pk * C.transpose() + R) " << inv(C * Pk * C.transpose() + R) << endl;
+                                                cout << "Pk * C.transpose() * inv(C * Pk * C.transpose() + R) " << Pk * C.transpose() * inv(C * Pk * C.transpose() + R) << endl;
+                                }*/
+                                /*KK = Pk * C.transpose() * inv(C * Pk * C.transpose() + R);               
+             
+                                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(4, k+1) = x(4, k);               
+
+                                x.set_col(k+1, x.get_col(k+1) - KK * (y.get_col(k) - x.get_col(k).get(0,1))); 
+                                randvec(0) = randn();
+                                randvec(1) = randn();
+                                y.set_col(k+1, x.get_col(k+1).get(0,1) + R * randvec);  
+
+                                A(2, 0) = -e*sin(x(3, k));
+                                A(2, 1) = e*cos(x(3, k));
+                                A(0, 2) = b*sin(x(3, k));
+                                A(1, 2) = -b*cos(x(3, k));
+                                A(0, 3) = b*(x(2, k) + x(4, k))*cos(x(3, k));
+                                A(1, 3) = b*(x(2, k) + x(4, k))*sin(x(3, k));    
+                                A(2, 3) = -e*(x(1, k)*sin(x(3, k) + x(0, k)*cos(x(3, k))));
+                                A(0, 4) = b*sin(x(3, k));
+                                A(1, 4) = -b*cos(x(3, k));     
+//cout << "A(" << k << "): " << endl << A << endl;
+                                Pk = A * (Pk - Pk * C.transpose() * inv(C * Pk * C.transpose() + R) * C * Pk) * A.transpose() + Q;              
+cout << "Pk(" << k << "): " << endl << Pk << endl;
+                        }
+                        
+            lq.resetTime();
+                        lq.redesign();
+            for(k = K-1; k > 0; k--){               
+                A(2, 0) = -e*sin(x(3, k+kt-1));
+                A(2, 1) = e*cos(x(3, k+kt-1));
+                A(0, 2) = b*sin(x(3, k+kt-1));
+                A(1, 2) = -b*cos(x(3, k+kt-1));
+                A(0, 3) = b*(x(2, k+kt-1) + x(4, k+kt-1))*cos(x(3, k+kt-1));
+                A(1, 3) = b*(x(2, k+kt-1) + x(4, k+kt-1))*sin(x(3, k+kt-1));    
+                A(2, 3) = -e*(x(1, k+kt-1)*sin(x(3, k+kt-1) + x(0,k+kt-1)*cos(x(3, k+kt-1))));
+                A(0, 4) = b*sin(x(3, k+kt-1));
+                A(1, 4) = -b*cos(x(3, k+kt-1));                                         
+                                lq.Models(0).A = A;                                                      
+                                lq.redesign();
+                        }
+                        L = lq.getL();
+                        L0.set_col(kt, L.get_row(0).get(0,4));
+                        L1.set_col(kt, L.get_row(1).get(0,4));
+                }        
+                
+        x.set_col(0, x00);
+                y.set_col(0, x.get_col(0).get(0,1));
+
+                loss = 0;
+
+        for(k = 0; k < Kt; k++){               
+                        L.set_row(0, L0.get_col(k));
+                        L.set_row(1, L1.get_col(k));
+                        u = L*x.get_col(k);     
+
+                        KK = Pk * C.transpose() * inv(C * Pk * C.transpose() + R);
+
+            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(4, k+1) = x(4, k); 
+
+                        loss += (x.get_col(k+1).transpose() * X * x.get_col(k+1).transpose())(0,0);
+                        loss += (u.transpose() * Y * u)(0);
+               
+            x.set_col(k+1, x.get_col(k+1) - KK * (y.get_col(k) - x.get_col(k).get(0,1))); 
+                        randvec(0) = randn();
+                        randvec(1) = randn();
+                        y.set_col(k+1, x.get_col(k+1).get(0,1) + R * randvec);                   
+
+            A(2, 0) = -e*sin(x(3, k));
+                        A(2, 1) = e*cos(x(3, k));
+                        A(0, 2) = b*sin(x(3, k));
+                        A(1, 2) = -b*cos(x(3, k));
+                        A(0, 3) = b*(x(2, k) + x(4, k))*cos(x(3, k));
+                        A(1, 3) = b*(x(2, k) + x(4, k))*sin(x(3, k));    
+                        A(2, 3) = -e*(x(1, k)*sin(x(3, k) + x(0, k)*cos(x(3, k))));
+                        A(0, 4) = b*sin(x(3, k));
+                        A(1, 4) = -b*cos(x(3, k));     
+
+                        Pk = A * (Pk - Pk * C.transpose() * inv(C * Pk * C.transpose() + R) * C * Pk) * A.transpose() + Q;                 
+                } 
+
+                losses(n) = loss;        
+       
+        }
+
+        cout << "Ztrata: " << sum(losses)/n << endl;
 }
 
-TEST (LQGU_large_test){
-        bdm_assert(0, "Test not implemented.");
-}
-
-TEST (validation_test){
+//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);
@@ -721 +1145 @@
         all.add(rv5);
         all.add(RV("1",1,0));
-        cout << "all rv: " << all << endl;
+        //cout << "all rv: " << all << endl;
 
         ivec fy = rv2.findself(all);
-        cout << "finding y: " << fy << endl;
+        //cout << "finding y: " << fy << endl;
         ivec dy = rv2.dataind(all);
-        cout << "dataind y: " << dy << endl;
+        //cout << "dataind y: " << dy << endl;
 
         RV rvzyu;
@@ -733 +1157 @@
         rvzyu.add(rv4);
         fy = rvzyu.findself(all);
-        cout << "finding zyu: " << fy << endl;
+        //cout << "finding zyu: " << fy << endl;
         dy = rvzyu.dataind(all);
-        cout << "dataind zyu: " << dy << endl;
+        //cout << "dataind zyu: " << dy << endl;
 
         rvzyu.add(RV("1",1,0));
         fy = rvzyu.findself(all);
-        cout << "finding zyu1: " << fy << endl;
+        //cout << "finding zyu1: " << fy << endl;
         dy = rvzyu.dataind(all);
-        cout << "dataind zyu1: " << dy << endl;
+        //cout << "dataind zyu1: " << dy << endl;
 
         rvzyu.add(RV("k",1,0));
         fy = rvzyu.findself(all);
-        cout << "finding zyu1 !k!: " << fy << endl;
+        //cout << "finding zyu1 !k!: " << fy << endl;
         dy = rvzyu.dataind(all);
-        cout << "dataind zyu1 !k!: " << dy << endl;
+        //cout << "dataind zyu1 !k!: " << dy << endl;
 
         RV emptyrv;
         fy = emptyrv.findself(all);
-        cout << "finding empty: " << fy << " size " << fy.size() << endl;
+        //cout << "finding empty: " << fy << " size " << fy.size() << endl;
         dy = emptyrv.dataind(all);
-        cout << "dataind empty: " << dy << " size " << dy.size() << endl;
+        //cout << "dataind empty: " << dy << " size " << dy.size() << endl;
 
         LQG_universal lq;
         lq.validate();
-}
+}*/