//---------------------------------------------------------------------------
//
//   Design of the UV-MRAC for a linear fourth order system of relative
//   degree one: three trailers linked by one damper and one spring.
//   The design is fault-tolerant.
//
//   Trailers are linked by one spring (trailers 1 and 3) and
//   one damper (trailers 2 and 3). Trailers 1 and 2 are active and,
//   trailer 3 is passive.
//
//   Developed for Scilab 2.6.
//
//   Rio de Janeiro, March 20, 2003.
//
//   Author: Jose' Paulo V. S. da Cunha
//
//---------------------------------------------------------------------------

// Load library:

exec("FOAF-tools.sci");

//--------------------------------------------------------------------------
//   Function for the design of UVC for multivariable plants of relative
//   degree one:
//
//   dot_e = Am e + Kp [u + d]
//
//       u = -rho e/||e||
//
//   The modulation function is given by:
//
//   rho=ce*||e||+(1+cd)*||d||+delta
//
//   Rio de Janeiro, March 20, 2003.
//
//   Author: Jose' Paulo V. S. da Cunha
//
//---------------------------------------------------------------------------
//
function [ce,cd]=uvc(Am,Kp,Q,delta_bar)
//
// UVC modulation function constants:     ce,cd>=0
// Plant  high frequency gain matrix:     Kp
// Reference model matrix:                Am
// Lyapunov design matrix:                Q=Q'>0
// Desired stability margin:              delta_bar

// Solution of Lyapunov equation:

P = lyap(Kp,Q,'c')

// Modulation function coefficients:

lmin=min(spec(Q))

cd = 2 * norm(P*Kp,2)/lmin - 1

ce = max(max(spec(Am'*P+P*Am))/lmin + delta_bar, 0)

endfunction

//---------------------------------------------------------------------------
//
//  Function to compute the plant matrices (A,B,C), as well as in partitioned
//  form (Aij), the high frequency gain matrix (Kp=M^-1SFS'Mnom) of the
//  chain of trailers system and its inverse (Kp1=KP^-1).
//
//  State vector: xp=[x1' v]', x1=[l31 v3]' and v=[v1 v2]', where vi is the
//  velocity of the i-th trailer and, l31 is the distance between trailers
//  3 and 1.
//
//   Last modified: March 20, 2003.
//
//   Author: Jose' Paulo V. S. da Cunha
//
//---------------------------------------------------------------------------
//

function [A,B,C,A11,A12,A21,A22,Kp,Kp1]=trailers(M,m3,k31,b23,F,Mnom)
//
// Inertia matrix                            : M (=diag(m1,m2))
// Inverse of the nominal inertia matrix     : Mnom1
// Mass of the passive trailer               : m3
// Linear dampers coefficient                : b23
// Linear spring  coefficient                : k31
// Fault indices (diag(F1,F2,F3))            : F
// Nominal inertia matrix                    : Mnom
//

// Inverse of the inertia matrix:

M1 = inv(M)

// Open-loop plant matrices:

A11 = [   0       1   ;
      -k31/m3 -b23/m3 ]

A12 = [  -1       0   ;
          0    b23/m3 ]

A21 = [  k31      0   ;
         0       b23  ] * M1

A22 = [  0        0   ;
         0      -b23  ] * M1

// S Matrix:

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

// Matrices of the linear part of the plant:

A = [ A11 A12 ;
      A21 A22 ]

B = [ zeros(2,3) ;
         M1*S    ]

C = [ zeros(2,2) eye(2,2) ]

// High frequency gain matrix including fault and mixer matrices:

Kp = M1*S*F*S'*Mnom

Kp1 = inv(Kp)

endfunction

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

//
// Trailer parameters:
//

// Trailer mass (kg):

m1 = 1;

m2 = 2;

// Uncertain trailer mass (kg):

m3table = [ 0.5 ;
             1  ;
            1.5 ] ;

// Spring constant (N/m):

k31 = 1;

// Damper constant (Ns/m):

b23 = 1;

// Inertia matrix:

M = diag([m1 m2]);

// Reference model matrix:

Am= [ -2  0 ;
       0 -2 ];

// Lyapunov design matrix:

Q = eye(2,2);

// Desired stability margin:

delta_bar = 0.1;

// Table of admissible actuators failures:

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

//
// Nominal parameters applied in the design:
//

// Precompensation matrix (nominal inertia matrix - kg):

Mnom = diag([1 2]);

// Mass of trailer 3 (kg):

m3nom = 1;

// Nominal fault matrix:

Fnom = diag([1 1 1]);

//
// Filter optimization parameters:
//

delta_u = 0.01;

delta_l = 0.01;

epsilon1 = 0.1;

epsilon2 = 0.01;

ho = 10;

//
//                                   DESIGN:
//

//  Nominal system matrices computation:

[Anom,Bnom,Cnom,A11nom,A12nom,A21nom,A22nom,Kpnom,Kp1nom]=trailers(M,m3nom,k31,b23,Fnom,Mnom);

vu_nom = Kp1nom * (Am - A22nom)

Kp1nom

Kp1A22nom = Kp1nom * A22nom;

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

// Initializing the UV-MRAC constants tables:

total = size(Ftable,'r')*size(m3table,'r');

c = zeros(total,6);

gamma_1 = zeros(total);

//
// Compute the constants for each admissible actuators failure and value
// for m3:
//

for j=1:size(m3table,'r'),
 for i=1:size(Ftable,'r'),

  item = i + (j-1)*size(Ftable,'r');

  [A,B,C,A11,A12,A21,A22,Kp,Kp1]=trailers(M,m3table(j),k31,b23,diag(Ftable(i,:)),Mnom);

  [ce,cd]=uvc(Am,Kp,Q,delta_bar);

  c(item,1) = ce;

  cd1 = cd + 1;

  Kpp = Kp1nom - Kp1;

  c(item,2) = cd1 * norm(Kpp * Am - Kp1A22nom + Kp1 * A22 ,2);

  c(item,3) = cd1 * norm(Kpp ,2);

  c(item,4) = cd1 * norm(Kp1 ,2);

  [c5,gamma1,c_gamma_max,gamma_max,c_delta_l,t,g_norm]=first_order_filter(A11,A12,Kp1*A21,delta_u,delta_l,epsilon1,epsilon2,ho);

  c(item,5) = c5;

  gamma_1(item) = gamma1;

  c(item,6) = 0;

 end;
end;

// Display the results:

gamma_1

c

// Sugested coefficient for finite-time convergence:

delta = 0.1