root/library/tests/testsuite/LQG_test.cpp @ 1215

#define BDMLIB
#include "../mat_checks.h"
#include "design/lq_ctrl.h"

using namespace bdm;

TEST ( LQG_test ) {
    LQG reg;
    shared_ptr<StateSpace<chmat> > stsp = new StateSpace<chmat>;
    // 2 x 1 x 1
    stsp-> set_parameters ( eye ( 2 ), ones ( 2, 1 ), ones ( 1, 2 ), ones ( 1, 1 ), /* Q,R */ eye ( 2 ), eye ( 1 ) );
    reg.set_system ( stsp ); // A, B, C
    reg.set_control_parameters ( eye ( 1 ), eye ( 1 ),  vec_1 ( 1.0 ), 6 ); //Qy, Qu, horizon
    reg.validate();

    reg.redesign();
    double reg_apply = reg.ctrlaction ( "0.5, 1.1", "0.0" ) ( 0 ); /*convert vec to double*/
    CHECK_CLOSE ( reg_apply, -0.248528137234392, 0.0001 );
}

TEST ( to_state_test ) {
    mlnorm<fsqmat> ml;
    mat A = "1.1, 2.3";
    ml.set_parameters ( A, vec_1 ( 1.3 ), eye ( 1 ) );
    RV yr = RV ( "y", 1 );
    RV ur = RV ( "u", 1 );
    ml.set_rv ( yr );
    yr.t_plus ( -1 );
    ml.set_rvc ( concat ( yr, ur ) );

    shared_ptr<StateCanonical > Stsp = new StateCanonical;
    Stsp->connect_mlnorm ( ml );

    /* results from
    [A,B,C,D]=tf2ss([2.3 0],[1 -1.1])
    */
    CHECK_CLOSE_EX ( Stsp->_A().get_row ( 0 ), vec ( "1.1" ), 0.0001 );
    CHECK_CLOSE_EX ( Stsp->_C().get_row ( 0 ), vec ( "2.53" ), 0.0001 );
    CHECK_CLOSE_EX ( Stsp->_D().get_row ( 0 ), vec ( "2.30" ), 0.0001 );
}

TEST ( to_state_arx_test ) {
    mlnorm<chmat> ml;
    mat A = "1.1, 2.3, 3.4";
    ml.set_parameters ( A, vec_1 ( 1.3 ), eye ( 1 ) );
    RV yr = RV ( "y", 1 );
    RV ur = RV ( "u", 1 );
    ml.set_rv ( yr );
    ml.set_rvc ( concat ( yr.copy_t ( -1 ), concat ( ur, ur.copy_t ( -1 ) ) ) );

    shared_ptr<StateFromARX> Stsp = new StateFromARX;
    RV xrv;
    RV urv;
    Stsp->connect_mlnorm ( ml, xrv, urv );

    /* results from
    [A,B,C,D]=tf2ss([2.3 0],[1 -1.1])
    */
    CHECK_CLOSE_EX ( Stsp->_A().get_row ( 0 ), vec ( "1.1, 3.4, 1.3" ), 0.0001 );
    CHECK_CLOSE_EX ( Stsp->_B().get_row ( 0 ), vec ( "2.3" ), 0.0001 );
    CHECK_CLOSE_EX ( Stsp->_C().get_row ( 0 ), vec ( "1, 0, 0" ), 0.0001 );
}

TEST ( arx_LQG_test ) {
    mlnorm<chmat> ml;
    mat A = "1.81, -.81, .00468, .00438";
    ml.set_parameters ( A, vec_1 ( 0.0 ), 0.00001*eye ( 1 ) );
    RV yr = RV ( "y", 1 );
    RV ur = RV ( "u", 1 );
    RV rgr = yr.copy_t ( -1 );
    rgr.add ( yr.copy_t ( -2 ) );
    rgr.add ( yr.copy_t ( -2 ) );
    rgr.add ( ur.copy_t ( -1 ) );
    rgr.add ( ur );

    ml.set_rv ( yr );
    ml.set_rvc ( rgr );
    ml.validate();

    shared_ptr<StateFromARX> Stsp = new StateFromARX;
    RV xrv;
    RV urv;
    Stsp->connect_mlnorm ( ml, xrv, urv );

    LQG L;
    L.set_system ( Stsp );
    L.set_control_parameters ( eye ( 1 ), sqrt ( 1.0 / 1000 ) *eye ( 1 ), vec_1 ( 0.0 ), 100 );
    L.validate();

    L.redesign();
    cout << L.to_string() << endl;
}

//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
  LQG_universal lq;
  lq.rv = RV("u", 2, 0);  
  lq.set_rvc(RV("x", 2, 0));
  lq.horizon = 10;

  /*
                model:      x = Ax + Bu                 time: 0..horizon
                loss:       l = x'Q'Qx + u'R'Ru         time: 0..horizon-1
                final loss: l = x'S'Sx                  time: horizon

                dim:    x: 2
                                u: 2

                A = [   2       -1       ]
                        [       0       0.5      ]
                
                B = [   1               -0.1    ]       
                        [       -0.2    2               ]

                Q = [   5       0       ]
                        [       0       1       ]

                R = [   0.01    0        ]
                        [       0               0.1      ]

                S = Q
  */

  mat mA("2 -1;0 0.5"); 
  mat mB("1 -0.1;-0.2 2"); 
  mat mQ("5 0;0 1"); 
  mat mR("0.01 0;0 0.1"); 
  mat mS = mQ;

  //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;*/

  //model
  Array<linfnEx> model(2);
  //model Ax part
  model(0).A = mA;
  model(0).B = vec("0 0");
  model(0).rv = RV("x", 2, 0);
  model(0).rv_ret = RV("x", 2, 1);
  //model Bu part
  model(1).A = mB;
  model(1).B = vec("0 0");
  model(1).rv = RV("u", 2, 0);
  model(1).rv_ret = RV("x", 2, 1);      
  //setup
  lq.Models = model;

  //loss
  Array<quadraticfn> loss(2);
  //loss x'Qx part
  loss(0).Q.setCh(mQ);
  loss(0).rv = RV("x", 2, 1);
  //loss u'Ru part
  loss(1).Q.setCh(mR);
  loss(1).rv = RV("u", 2, 0);
  //setup
  lq.Losses = loss;

  //finalloss setup
  lq.finalLoss.Q.setCh(mS);
  lq.finalLoss.rv = RV("x", 2, 1);

  lq.validate();
  
  //default L
  //cout << "default L matrix:" << endl << lq.getL() << endl;

  //produce last control matrix L
  lq.redesign();
  
  //verification via Riccati LQG version
  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
  double tolerr = 1;//0.01; //0.01 OK x 0.001 NO OK

  //check last time L matrix
  CHECK_CLOSE_EX(lq.getL().get_cols(0,1), mL, tolerr);
  
  mat oldK;
  int i;

  //produce next control matrix L
  for(i = 0; i < lq.horizon - 1; i++) {
          lq.redesign();
          oldK = mK;
          mK = mA.transpose() * (oldK - oldK * mB * inv(mR + mB.transpose() * oldK * mB) * mB.transpose() * oldK) * mA + mQ;
          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);
  }

 //check losses of LQG control - compare LQG_universal and Riccati version, no noise
    
  //loss of LQG_universal
  /*double*/vec loss_uni("0");

  //setup
  vec x = x0;
  vec xold = x;
  vec u;
  //vec tmploss;

  //iteration
  for(i = 0; i < lq.horizon - 1; i++){
        u = lq.getL().get_cols(0,1) * xold;
        x = mA * xold + mB * u;
        /*tmploss*/loss_uni = x.transpose() * mQ * x + u.transpose() * mR * u;
        //loss_uni += tmploss.get(0);
        xold = x;
  }
  /*tmploss*/ loss_uni = x.transpose() * mS * x;
  //loss_uni += tmploss.get(0);

  //loss of LQG Riccati version
  /*double*/ vec loss_rct("0");

  //setup
  x = x0;
  xold = x;

  //iteration
  for(i = 0; i < lq.horizon - 1; i++){
        u = mL * xold;
        x = mA * xold + mB * u;
        /*tmploss*/loss_rct = x.transpose() * mQ * x + u.transpose() * mR * u;
        //loss_rct += tmploss.get(0);
        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;
  CHECK_CLOSE_EX(loss_uni, loss_rct, 0.0001);  
}

TEST (LQGU_random_vs_Riccati_test){
    
  //uniform random generator
  Uniform_RNG urng;
  double maxmult = 10.0;
  int maxxsize = 5;
  int maxusize = 5;

  urng.setup(2.0, maxxsize);  
  int matxsize = round_i(urng());
  urng.setup(2.0, maxusize);  
  int matusize = round_i(urng());

  urng.setup(-maxmult, maxmult);        

  mat tmpmat;
    
  mat mA;   
        mA = urng(matxsize, matxsize);  
  mat mB; 
        mB = urng(matxsize, matusize);  
  mat mQ; 
        tmpmat = urng(matxsize, matxsize);
        mQ = tmpmat*tmpmat.transpose();
  mat mR; 
        tmpmat = urng(matusize, matusize);
        mR = tmpmat*tmpmat.transpose();
  mat mS; 
        tmpmat = urng(matxsize, matxsize);
        mS = tmpmat*tmpmat.transpose();

  //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;*/
 
  vec zerovecx(matxsize);
        zerovecx.zeros();
  vec zerovecu(matusize);
        zerovecu.zeros();

//test of universal LQG controller
  LQG_universal lq;
  lq.rv = RV("u", matusize, 0);  
  lq.set_rvc(RV("x", matxsize, 0));
  lq.horizon = 10;

  //model
  Array<linfnEx> model(2);
  //model Ax part
  model(0).A = mA;
  model(0).B = zerovecx;
  model(0).rv = RV("x", matxsize, 0);
  model(0).rv_ret = RV("x", matxsize, 1);
  //model Bu part
  model(1).A = mB;
  model(1).B = zerovecu;
  model(1).rv = RV("u", matusize, 0);
  model(1).rv_ret = RV("x", matxsize, 1);       
  //setup
  lq.Models = model;

  //loss
  Array<quadraticfn> loss(2);
  //loss x'Qx part
  loss(0).Q.setCh(mQ);
  loss(0).rv = RV("x", matxsize, 1);
  //loss u'Ru part
  loss(1).Q.setCh(mR);
  loss(1).rv = RV("u", matusize, 0);
  //setup
  lq.Losses = loss;

  //finalloss setup
  lq.finalLoss.Q.setCh(mS);
  lq.finalLoss.rv = RV("x", matxsize, 1);

  lq.validate();  

  //produce last control matrix L
  lq.redesign();
   
  //verification via Riccati LQG version
  mat mK = mS;
  mat mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA;  

  //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

  //check last time L matrix
  CHECK_CLOSE_EX(lq.getL().get_cols(0,(matxsize-1)), mL, tolerr);
  
  mat oldK;
  int i;

  //produce next control matrix L
  for(i = 0; i < lq.horizon - 1; i++) {
          lq.redesign();
          oldK = mK;
          mK = mA.transpose() * (oldK - oldK * mB * inv(mR + mB.transpose() * oldK * mB) * mB.transpose() * oldK) * mA + mQ;
          mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA;   

          //check other times L matrix
          CHECK_CLOSE_EX(lq.getL().get_cols(0,(matxsize-1)), mL, tolerr);
  }

 //check losses of LQG control - compare LQG_universal and Riccati version, no noise
    
  //loss of LQG_universal
  vec loss_uni("0");

  //setup
  vec x = x0;
  vec xold = x;
  vec u;
  //vec tmploss;

  //iteration
  for(i = 0; i < lq.horizon - 1; i++){
        u = lq.getL().get_cols(0,(matxsize-1)) * xold;
        x = mA * xold + mB * u;
        loss_uni = x.transpose() * mQ * x + u.transpose() * mR * u;     
        xold = x;
  }

  loss_uni = x.transpose() * mS * x;
  
  //loss of LQG Riccati version
  vec loss_rct("0");

  //setup
  x = x0;
  xold = x;

  //iteration
  for(i = 0; i < lq.horizon - 1; i++){
        u = mL * xold;
        x = mA * xold + mB * u;
        loss_rct = x.transpose() * mQ * x + u.transpose() * mR * u;     
        xold = x;
  }
  loss_rct = x.transpose() * mS * x;  
  
  CHECK_CLOSE_EX(loss_uni, loss_rct, 0.0001);  
}

TEST (LQGU_SiSy_test){
    int K = 5;
    int N = 100;    
    double sigma = 0.1;        
    double yr = 1;
        double y0 = 0;
    double x0 = y0 - yr;
        double Eb = 1;
        double P;
        //double P = 0.01;      //loss ~ 1.0???         e-2
        //double P = 0.1;       //loss ~ 1.???          e-1
        //double P = 1;         //loss ~ ???.???        e+2
        //double P = 10;        //loss ~ ?e+6
               
    mat A("1"); 
    mat B(1,1);
                B(0,0) = Eb;
    mat Q("1");
    mat R("0");   
    
    vec u(K);
                u.zeros();
    mat xn(K, N);
                xn.zeros();
    vec S(K);            
                S.zeros();
    vec L(K);        
        L.zeros();

    vec loss(N); 
                loss.zeros();  

        LQG_universal lq;
        lq.rv = RV("u", 1, 0);  
        lq.set_rvc(RV("x", 1, 0));
        lq.horizon = K;

        Array<linfnEx> model(2);  
  model(0).A = A;
  model(0).B = vec("0 0");
  model(0).rv = RV("x", 1, 0);
  model(0).rv_ret = RV("x", 1, 1);  
  model(1).A = B;
  model(1).B = vec("0 0");
  model(1).rv = RV("u", 1, 0);
  model(1).rv_ret = RV("x", 1, 1);  
  lq.Models = model;

  Array<quadraticfn> qloss(2);  
  qloss(0).Q.setCh(Q);
  qloss(0).rv = RV("x", 1, 1);  
  qloss(1).Q.setCh(R);
  qloss(1).rv = RV("u", 1, 0);  
  lq.Losses = qloss;
  
  lq.finalLoss.Q.setCh(Q);
  lq.finalLoss.rv = RV("x", 1, 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);
                }

for(P = 0.01; P <= 10; (P*=10)){
        lq.resetTime();
        
//cout << "Ls " << L << endl;
        for(n = 0; n < N; n++){        
        Br = Eb + sqrt(P)*randn();        
   
                xn(0, n) = x0 + sigma*randn();            
                                for(k = 0; k < K-1; k++){               
                   u(k) = L(k)*xn(k, n);                     
                   xn(k+1, n) = (A*xn(k, n))(0,0) + Br*u(k) + sigma*randn();                                       
                                }
                
                loss(n) = 0;
                                for(k=0;k<K;k++) loss(n) += xn(k, n)*xn(k, n);
                
        }
    
        /*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
        //double P = 0.1;       //loss ~ 1.???          e-1
        //double P = 1;         //loss ~ ???.???        e+2
        //double P = 10;        //loss ~ ?e+6
        if(P == 0.01) tolerr = 0.2;
        else if(P == 0.1) tolerr = 2;
        else if(P == 1) tolerr = 2000;
        else if(P == 10) tolerr = 2e+7;
        else tolerr = 0;

        CHECK_CLOSE_EX(1.0, sum(loss)/N, tolerr);
        }
}

TEST (LQGU_SiSy_SuSt_test){
        int K = 5;
    int N = 100;    
    double sigma = 0.1;        
    double yr = 1;
        double y0 = 0;    
        double Eb = 1;
        double inP;
        //double inP = 0.01;    //loss ~ 1.0???         e-2
        //double inP = 0.1;     //loss ~ 1.0???         e-2
        //double inP = 1;               //loss ~ 1.0???         e-2
        //double inP = 10;      //loss ~ 1.0???         e-2

        vec x0(3);
                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");
        mat X("1 0 0; 0 0 0; 0 0 0");   
        mat Y("0.00001");
    mat Q(3,3);
                Q.zeros();
                Q(1,1) = sigma*sigma;
    //mat R("0");   
        //mat P(3,3);
        //      P.zeros();
    
    vec u(K);
                u.zeros();
        vec Kk(K);
                Kk.zeros();
    mat xn0(K+1, N);
                xn0.zeros();
        mat xn1(K+1, N);
                xn1.zeros();
        mat xn2(K+1, N);
                xn2.zeros();
    //vec S(K);            
        //      S.zeros();
    //vec L(K);        
    //    L.zeros();
        mat L(1, 3);
                L.zeros();
    vec loss(N); 
                loss.zeros();  

        LQG_universal lq;
        lq.rv = RV("u", 1, 0);  
        lq.set_rvc(RV("x", 3, 0));
        lq.horizon = K;

        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;
  
  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();
        x0(2) = inP;
        for(n = 0; n < N; n++){  
                L.zeros();
        xn0(0, n) = x0(0);           
                xn1(0, n) = x0(1) + sqrt(inP) * randn();            
                xn2(0, n) = x0(2); 
                //^neznalost, sum zatim ne

                                for(k = 0; k < K-1; k++){               
                   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);                                       
                                }
                
                                lq.resetTime();
                                lq.redesign();
                                for(k = K-1; k > 0; k--){               
                                        A(0, 1) = u(k);
                    A(1, 0) = -Kk(k);
                    A(1, 1) = 1-Kk(k)*u(k);
                    A(1, 2) = u(k)*sigma*sigma*(xn0(k+1, n) - xn0(k, n) - xn1(k, n)*u(k)) / sqr(u(k)*u(k)*xn2(k, n) + sigma*sigma);
                    A(2, 2) = 1 - u(k)*u(k)*xn2(k, n)*(u(k)*u(k)*xn2(k, n) + 2*sigma*sigma) / sqr(u(k)*u(k)*xn2(k, n) + sigma*sigma);
                    B(0) = xn1(k, n);
                    B(1) = ((xn2(k, n)*sigma*sigma-u(k)*u(k)*xn2(k, n)*xn2(k, n))*(xn0(k+1, n)-xn0(k, n)) - 2*xn1(k, n)*u(k)) / sqr(u(k)*u(k)*xn2(k, n) + sigma*sigma);
                    B(2) = -2*u(k)*xn2(k, n)*xn2(k, n)*sigma*sigma / sqr(u(k)*u(k)*xn2(k, n) + sigma*sigma);
                                        lq.Models(0).A = A;
                    lq.Models(1).A = B;                                      
                                        lq.redesign();
                                }
                                L = lq.getL();

                                //simulation
                                xn0(0, n) += sigma*randn(); 
                                for(k = 0; k < K-1; k++){
                                   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);                                       
                                }


                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);
        }
}

TEST (LQGU_PMSM_test){
        
        int K = 5;
        int Kt = 200;
        int N = 15;

        double a = 0.9898;
        double b = 0.0072;
        double c = 0.0361;
        double d = 1;
        double e = 0.0149; 

        double OMEGAt = 11.5;
        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 u(2);
        
        mat x(5,Kt+K);  

        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();
        vec omega(Kt+K);
                omega.zeros();
        
        for(n = 0; n < N; n++) {
                L0.zeros();
                L1.zeros();
                                                    
                for(i=0;i<5;i++) x00(i) = x0(i) + sqrt(P(i,i))*randn();   
                       
                for(kt = 0; kt < Kt; kt++){            
                        x.set_col(0, x00);

                        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(u(0) > 50) u(0) = 50;
                                else if(u(0) < -50) u(0) = -50;
                                if(u(1) > 50) u(1) = 50;
                                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(4, k+1) = x(4, k);                            
                        }
                        
            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);
                
        for(k = 0; k < Kt; k++){ 
        
                        L.set_size(2,5);
                        L.set_row(0, L0.get_col(k));
                        L.set_row(1, L1.get_col(k));
                        u = L*x.get_col(k);     
                        if(u(0) > 50) u(0) = 50;
                        else if(u(0) < -50) u(0) = -50;
                        if(u(1) > 50) u(1) = 50;
                        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(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);                      
                }

                omega += x.get_row(2);
        }

        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();

}

TEST (LQGU_181_zero){
        /*                                               v ????? .8189
                y_t = 1.81 y_{t-1} -.9 y_{t-2} + .00438 u_t + .00468 u_{t-1}

                S = y^2 + u^2/1000

                y_r = 0  :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2}  -0.2509 u_{t-1}
                y_r = 10 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2}  -0.2509 u_{t-1} + 233.7581
        */

        int K = 150;

        LQG_universal lq;
        lq.rv = RV("ut", 1, 0); 
        RV rvc; 
        rvc = RV("yt", 1, -1);  
        rvc.add(RV("yt", 1, -2));
        rvc.add(RV("ut", 1, -1));       
        rvc.add(RV("1", 1, 0));
        lq.set_rvc(rvc);
        lq.horizon = K;

        Array<linfnEx> model(4);  
        model(0).A = mat("1.81");
        model(0).B = vec("0");
        model(0).rv = RV("yt", 1, -1);
        model(0).rv_ret = RV("yt", 1, 0);        
        model(1).A = mat("-0.8189");
        model(1).B = vec("0");
        model(1).rv = RV("yt", 1, -2);
        model(1).rv_ret = RV("yt", 1, 0);       
        model(2).A = mat("0.00438");
        model(2).B = vec("0");
        model(2).rv = RV("ut", 1, 0);
        model(2).rv_ret = RV("yt", 1, 0);       
        model(3).A = mat("0.00468");
        model(3).B = vec("0");
        model(3).rv = RV("ut", 1, -1);
        model(3).rv_ret = RV("yt", 1, 0);       
        lq.Models = model;

        Array<quadraticfn> qloss(2);  
        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).rv = RV("ut", 1, 0);   
        lq.Losses = qloss;
 
        lq.finalLoss.Q.setCh(mat("1"));
        lq.finalLoss.rv = RV("yt", 1, 0);       

        lq.validate(); 

        lq.resetTime();
        lq.redesign();

        for(int k = K-1; k > 0; k--){
                //cout << "L: " << lq.getL() << endl;
                lq.redesign();
        }

        cout << "L: " << lq.getL() << endl;
}

TEST (LQGU_181_ten){
        /*                                               v ????? .8189
                y_t = 1.81 y_{t-1} -.9 y_{t-2} + .00438 u_t + .00468 u_{t-1}

                S = y^2 + u^2/1000

                y_r = 0  :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2}  -0.2509 u_{t-1}
                y_r = 10 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2}  -0.2509 u_{t-1} + 233.7581
        */

        int K = 150;
        //yr = 10.0;

        LQG_universal lq;
        lq.rv = RV("ut", 1, 0); 
        RV rvc; 
        rvc = RV("yt", 1, -1);  
        rvc.add(RV("yt", 1, -2));
        rvc.add(RV("ut", 1, -1));       
        rvc.add(RV("1", 1, 0));
        lq.set_rvc(rvc);
        lq.horizon = K;

        Array<linfnEx> model(4);  
        model(0).A = mat("1.81");
        model(0).B = vec("0");
        model(0).rv = RV("yt", 1, -1);
        model(0).rv_ret = RV("yt", 1, 0);        
        model(1).A = mat("-0.8189");
        model(1).B = vec("0");
        model(1).rv = RV("yt", 1, -2);
        model(1).rv_ret = RV("yt", 1, 0);       
        model(2).A = mat("0.00438");
        model(2).B = vec("0");
        model(2).rv = RV("ut", 1, 0);
        model(2).rv_ret = RV("yt", 1, 0);       
        model(3).A = mat("0.00468");
        model(3).B = vec("0");
        model(3).rv = RV("ut", 1, -1);
        model(3).rv_ret = RV("yt", 1, 0);       
        lq.Models = model;

        Array<quadraticfn> qloss(2);  
        qloss(0).Q.setCh(mat("1 -10"));
        qloss(0).rv = RV("yt", 1, 0);   
        qloss(0).rv.add(RV("1", 1, 0));
        qloss(1).Q.setCh(mat("0.03162"));       
        qloss(1).rv = RV("ut", 1, 0);   
        lq.Losses = qloss;
 
        lq.finalLoss.Q.setCh(mat("0"));
        lq.finalLoss.rv = RV("yt", 1, 0);       

        lq.validate(); 

        lq.resetTime();
        lq.redesign();

        for(int k = K-1; k > 0; k--){
                //cout << "L: " << lq.getL() << endl;
                lq.redesign();
        }

        cout << "L: " << lq.getL() << endl;
}

TEST (LQGU_181_reqv){
        /*                                               v ????? .8189
                y_t = 1.81 y_{t-1} -.9 y_{t-2} + .00438 u_t + .00468 u_{t-1}

                S = y^2 + u^2/1000

                y_r = 0  :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2}  -0.2509 u_{t-1}
                y_r = 10 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2}  -0.2509 u_{t-1} + 233.7581
        */

        int K = 200;

        double yr = 10.0;

        LQG_universal lq;
        lq.rv = RV("ut", 1, 0); 
        RV rvc; 
        rvc = RV("yt", 1, -1);  
        rvc.add(RV("yt", 1, -2));
        rvc.add(RV("ut", 1, -1));
        rvc.add(RV("yr", 1, 0));
        rvc.add(RV("1", 1, 0));
        lq.set_rvc(rvc);
        lq.horizon = K;

        Array<linfnEx> model(5);  
        model(0).A = mat("1.81");
        model(0).B = vec("0");
        model(0).rv = RV("yt", 1, -1);
        model(0).rv_ret = RV("yt", 1, 0);        
        model(1).A = mat("-0.8189");
        model(1).B = vec("0");
        model(1).rv = RV("yt", 1, -2);
        model(1).rv_ret = RV("yt", 1, 0);       
        model(2).A = mat("0.00438");
        model(2).B = vec("0");
        model(2).rv = RV("ut", 1, 0);
        model(2).rv_ret = RV("yt", 1, 0);       
        model(3).A = mat("0.00468");
        model(3).B = vec("0");
        model(3).rv = RV("ut", 1, -1);
        model(3).rv_ret = RV("yt", 1, 0);       
        model(4).A = mat("1");
        model(4).B = vec("0");
        model(4).rv = RV("yr", 1, 0);
        model(4).rv_ret = RV("yr", 1, 1);
        lq.Models = model;

        Array<quadraticfn> qloss(2);  
        qloss(0).Q.setCh(mat("1 -1"));
        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).rv = RV("ut", 1, 0);   
        lq.Losses = qloss;
 
        lq.finalLoss.Q.setCh(mat("1 -1"));
        lq.finalLoss.rv = RV("yt", 1, 0);       
        lq.finalLoss.rv.add(RV("yr", 1, 0));    

        lq.validate(); 

        lq.resetTime();
        lq.redesign();

        for(int k = K-1; k > 0; k--){
                //cout << "L: " << lq.getL() << endl;
                lq.redesign();
        }

        cout << "L: " << lq.getL() << 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();
}*/