//---------------------------------------------------------------------------
//
//   Design of the UV-MRAC for a nonlinear third order system of relative
//   degree one: three trailers linked by nonlinear dampers. The design is
//   fault-tolerant.
//
//   Developed for Scilab 2.6.
//
//   Rio de Janeiro, January 10, 2003.
//
//   Author: Jose' Paulo V. S. da Cunha
//
//   Revised in April 22, 2003.
//
//---------------------------------------------------------------------------

// Load libraries:

exec("FOAF-tools.sci");

exec("MRCtools.sci");

//---------------------------------------------------------------------------
//
//  Function to compute the plant matrices (Ap,Bp,Cp), open loop matrices
//  (Ao,Bo,Co,Bphi), error equation matrices (Ac,Bc,Co,Bphi), paramater
//  matrices (theta,theta4), right matrix fraction decomposition of the
//  open loop plant transfer function (Nr(s),Dr(s)), state filters transfer
//  function (Lambda(s),A(s)) and nonlinear disturbance upper bound
//  coefficients (kx,varphi) of the chain of trailers system.
//
//   Last modified: January 08, 2003.
//
//   Author: Jose' Paulo V. S. da Cunha
//
//---------------------------------------------------------------------------
//

function [Ap,Bp,Cp,Ao,Bo,Co,Bphi,Ac,Bc,Nr,Dr,Lambda,A,theta,theta4,kx,varphi]=trailers(M,B31,B23,F,k_damper,varphi_damper,Am,PHI,GAMMA)
//
// Inertia matrix                            : M (=diag(m1,m2,m3))
// Linear dampers coefficients               : B31,B23
// Fault indices (diag(F1,F2,F3))            : F
// Nonlinear dampers upper bounds            : |Fd|<=k_damper|v|+varphi_damper
// Reference model matrix                    : Am
// State filters matrices                    : PHI,GAMMA
//

// Laplace complex variable definition:

s=poly(0,"s")

// Inverse of the inertia matrix:

M1 = inv(M)

// Nonlinearity upper bound coefficients:

kx = norm(M1*[-k_damper 0 k_damper;
               0 -k_damper k_damper;
               k_damper k_damper -(k_damper+k_damper)])

varphi = norm(M1*[varphi_damper;
                  varphi_damper;
                  (varphi_damper+varphi_damper)])

// S Matrix:

S = eye(2,3)-[zeros(2,1) eye(2,2)]

// Matrices of the linear part of the plant:

Ap = M1*[-B31 0 B31; 0 -B23 B23; B31 B23 -(B31+B23)]

Bp = M1*[1 0; 0 1; 0 0]*S*F*(S')

Cp = [1 0 0; 0 1 0]

// Nominal plant right matrix fraction decomposition (G(s)=Nr(s)(Dr(s))^-1):

[n,m] = size(Bp)

[N,d]=coff(Ap)

Nr=Cp*N*Bp

Dr= d*eye(m,m)

// Open loop plant plus filters in state space:

[lGAMMA,cGAMMA] = size(GAMMA)

Ao = [Ap zeros(n,2*lGAMMA) ; zeros(lGAMMA,n) PHI zeros(lGAMMA,lGAMMA) ;
      GAMMA*Cp zeros(lGAMMA,lGAMMA) PHI]

Bo = [ Bp ; GAMMA ; zeros(lGAMMA,m) ]

Bphi = [ eye(n,n) ; zeros(2*lGAMMA,n) ]

Co = [ Cp zeros(m,2*lGAMMA) ]

// State filters transfer function matrix form:

[N,d]=coff(PHI)

A = N*GAMMA

Lambda = d*eye(cGAMMA,cGAMMA)

[lA,cA] = size(A)

//  Parameter matrix (theta_null is a dummy matrix):

[theta,theta_null]=mrcmatch(Nr,Dr,(s*eye(m,m)-Am),A,Lambda)

theta4 = theta(2*lGAMMA+m+1:2*(lGAMMA+m),:)

theta = theta(1:2*lGAMMA+m,:)

// Closed loop error equation matrices:

Omega1 = [ zeros(lGAMMA,n) eye(lGAMMA,lGAMMA) zeros(lGAMMA,lGAMMA) ;
           zeros(lGAMMA,n) zeros(lGAMMA,lGAMMA) eye(lGAMMA,lGAMMA) ;
           Cp              zeros(m,2*lGAMMA) ]

Ac = Ao+Bo*(theta')*Omega1

Bc = Bo*(theta4')

endfunction

//--------------------------------------------------------------------
//
//                         Begining of design.
//
//--------------------------------------------------------------------

// Laplace complex variable definition:

s=poly(0,"s");

// Precompensation matrix:

Sp = eye(2,2);

// Reference model matrix:

Am= [-4 0; 0 -4];

// State filters state space form:

PHI = [-10 0;0 -10];

GAMMA = [1 0; 0 1];

// Lyapunov design matrix:

Q = eye(2,2);

// Desired stability margin:

delta_bar = 0.1;

//
//                                PLANT PARAMETERS:
//

// Nominal inertia matrix (kg):

M_nominal = diag([1 2 0.5]);

// Actual inertia matrix (kg):

M = M_nominal;

// Nominal linear dampers coefficients (Ns/m):

B31 = 1;

B23 = 1;

// Nonlinearity upper bound coefficients:

k_damper = 0.1875;

varphi_damper = 0.5;

// Table of admissible actuators failures:

Ftable = [1,1,1;
          1,1,0;
          1,0,1;
          0,1,1];

//
// Filter optimization parameters:
//

delta_u = 0.01;

delta_l = 0.01;

epsilon1 = 0.1;

epsilon2 = 0.01;

ho = 10;

//
//                                   DESIGN:
//

//  Nominal parameter matrix computation:

[Ap,Bp,Cp,Ao,Bo,Co,Bphi,Ac,Bc,Nr,Dr,Lambda,A,theta_nom,theta4_nom,kx,Varphi]=trailers(M_nominal,B31,B23,eye(3,3),k_damper,varphi_damper,Am,PHI,GAMMA);

theta_nom

theta4_nom

norm_theta4_nom = norm(theta4_nom,2);

//adopted_theta_nom = zeros(2*(m+lA),m);
//adopted_theta_nom = theta_nom;

//
// Design of the UV-MRAC:
//

// Initializing the UV-MRAC constants tables:

c = zeros(4,11);

Gamma = zeros(4);

gamma_x = zeros(4);

gamma_phi = zeros(4);

k_x = zeros(4);

varphi = zeros(4);

//
// Compute the constants for each admissible actuators failure:
//

[n,m] = size(Bp);

[lGAMMA,cGAMMA] = size(GAMMA);

for i=1:4,

  [Ap,Bp,Cp,Ao,Bo,Co,Bphi,Ac,Bc,Nr,Dr,Lambda,A,theta,theta4,kx,Varphi]=trailers(M,B31,B23,diag(Ftable(i,:)),k_damper,varphi_damper,Am,PHI,GAMMA);

  varphi(i) = Varphi;

  k_x(i) = kx;

  [c1,c2,c3,cd,Kp] = uvmrac13(Nr,Dr,Am,Lambda,A,[theta_nom;theta4_nom],Sp,Q,delta_bar);

  c(i,1) = c1;

  c(i,2) = c2;

  c(i,3) = c3;

  K = Kp*Sp;

  K1 = (1+cd)*inv(K);

  c(i,4) = norm(K1*Cp);

  [c5,gammaphi,c_gamma_max,gamma_max,c_delta_l,t,g_norm]=first_order_filter(Ac,Bphi,K1*(Co*Ac-Am*Co),delta_u,delta_l,epsilon1,epsilon2,ho);

  c(i,5) = c5;

  gamma_phi(i) = gammaphi;

//  c(i,6) = norm(Bo,2);
  c(i,6) = norm(Bo*Sp,2);

  [c_phi,GAmma,c_gamma_max,gamma_max,c_delta_l,t,g_norm]=first_order_filter(Ac,Bphi,eye(n+2*lGAMMA,n+2*lGAMMA),delta_u,delta_l,epsilon1,epsilon2,ho);

  Gamma(i) = GAmma;

  gamma_x(i) = Gamma(i) - c_phi*kx;

  c(i,7) = c_phi;

  [c_dummy,Gamma_dummy,c_gamma_max,gamma_max,c_u,t,g_norm]=first_order_filter(Ac,Bo,eye(n+2*lGAMMA,n+2*lGAMMA),delta_u,Gamma(i),epsilon1,epsilon2,ho);

//  c(i,8) = c_u;
  c(i,8) = c_u * norm(Sp,2);
  
//  [c_dummy,Gamma_dummy,c_gamma_max,gamma_max,c_tau,t,g_norm]=first_order_filter(Ac,Ac*Bo,eye(n+2*lGAMMA,n+2*lGAMMA),delta_u,Gamma(i),epsilon1,epsilon2,ho);
  [c_dummy,Gamma_dummy,c_gamma_max,gamma_max,c_tau,t,g_norm]=first_order_filter(Ac,Ac*Bo*Sp,eye(n+2*lGAMMA,n+2*lGAMMA),delta_u,Gamma(i),epsilon1,epsilon2,ho);

  c(i,9) = c_tau + c_phi*kx*c(i,6);

  c(i,10) = c_u*norm(theta-theta_nom,2);

  c(i,11) = c_u*norm_theta4_nom;

end;

// Display the results:

  gamma_x

  gamma_phi

  k_x

  varphi

  c

// Sugested coefficient for finite-time convergence:

delta = 0.1

// Suggested average filters time constant:

tau = 0.01