//---------------------------------------------------------------------------
//
// Functions for solving the model matching problem in model-reference
// control (MRC) and for the design of the UV-MRAC.
//
//
//   Rio de Janeiro, December 19, 2001.
//
//   Author: Jose' Paulo V. S. da Cunha
//
//   Developed for Scilab version 2.6.
//
//  Last revision: April 04, 2003.
//
//---------------------------------------------------------------------------

//---------------------------------------------------------------------------
//
//  Function for the computation of the the ideal parameter matrix
//  in a multivariable model-reference control system. If the solution
//  is unique then theta1=0. If the solution is nonunique, then theta_null
//  returns the nullspace of the complete solution.
//
//  Author: Jose' Paulo V. S. da Cunha
//  Last revision: December 02, 2001.
// 
//---------------------------------------------------------------------------
//
function [theta,theta_null]=mrcmatch(Nr,Dr,Wm1,A,Lambda)
//
// Ideal parameter matrix for perfect matching      : theta
// Basis for the null space of the solution         : theta_null
// Right matrix fraction decomposition of the plant : Nr, Dr (G(s)=Nr(s)(Dr(s))^-1)
// Right matrix fraction decomposition
// of the reference model                           : Wm1 (Wm(s)=(Wm1(s))^-1)
// Right matrix fraction decomposition
// of the state filters                             : A, Lambda (A(s)(Lambda(s))^-1)

// Generation of matrices of coefficients of the terms of the Diophantine equation:

right=coeff(Lambda*Dr)
left1=coeff(A*Dr)
left2=coeff(A*Nr)
left3=coeff(Lambda*Nr)
left4=coeff(Lambda*Wm1*Nr)

// Equalization of the number of columns of the coeeficient matrices:

[ll1,cl1]=size(left1)
[ll2,cl2]=size(left2)
[ll3,cl3]=size(left3)
[ll4,cl4]=size(left4)
[lr,cr]=size(right)
col=max(cl1,cl2,cl3,cl4,cr)

if col>cl1 then
   left1=[left1 zeros(ll1,col-cl1)]
end

if col>cl2 then
   left2=[left2 zeros(ll2,col-cl2)]
end

if col>cl3 then
   left3=[left3 zeros(ll3,col-cl3)]
end

if col>cl4 then
   left4=[left4 zeros(ll4,col-cl4)]
end

if col>cr then
   right=[right zeros(lr,col-cr)]
end

// Obtaining the matrices of the matching equation left*theta = right

left=[left1' left2' left3' left4']
right=right'

// Solving the first column of the matching equation:

[theta,theta_null]=linsolve(left,-right(:,1))

// Solving the remaining columns of the matching equation:

if lr>1 then
  for i=2:lr,
    [theta_i,theta_null]=linsolve(left,-right(:,i))
    theta=[theta theta_i]
  end
end

endfunction

//--------------------------------------------------------------------------
// Function for the design of the UV-MRAC for multivariable plants of
// relative degree one.
//
//  Author: Jose' Paulo V. S. da Cunha
//  Last revision: December 19, 2001.
// 
//---------------------------------------------------------------------------
//
function [c1,c2,cd,Kp]=uvmrac1(Nr,Dr,Am,Lambda,A,theta_nom,Sp,Q,delta_bar)
//
// UV-MRAC modulation function constants: c1,c2,cd>=0
// Plant  high frequency gain matrix:     Kp
// Reference model matrix:                Am
// Nominal parameter matrix:              theta_nom
// Precompensation matrix:                Sp
// Lyapunov design matrix:                Q=Q'>0
// Desired stability margin:              delta_bar

// Computation of the ideal parameter matrix (theta):

[lAm,cAm]=size(Am)

[theta,theta_null]=mrcmatch(Nr,Dr,(poly(0,"s")*eye(lAm,cAm)-Am),A,Lambda)

// Computation of the plant high frequency gain matrix (Kp):

[lt,ct]=size(theta)

Kp=(inv(theta(lt-ct+1:lt,:)))'

K=Kp*Sp

// Solution of Lyapunov equation:

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

// Modulation function coefficients:

lmin=min(spec(Q))

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

c1 = 2 * norm(P*Kp*(theta_nom-theta)')/lmin

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

endfunction

//--------------------------------------------------------------------------
// Function for the design of the UV-MRAC for multivariable plants of
// relative degree one. This version also computes c3 and returns theta.
//
//  Author: Jose' Paulo V. S. da Cunha
//  Last revision: April 04, 2003.
// 
//---------------------------------------------------------------------------
//
function [c1,c2,c3,cd,theta,Kp]=uvmrac2(Nr,Dr,Am,Lambda,A,theta_nom,Sp,Q,delta_bar)
//
// UV-MRAC modulation function constants: c1,c2,c3,cd>=0
// Ideal parameter matrix:                theta
// Plant  high frequency gain matrix:     Kp
// Reference model matrix:                Am
// Nominal parameter matrix:              theta_nom
// Precompensation matrix:                Sp
// Lyapunov design matrix:                Q=Q'>0
// Desired stability margin:              delta_bar

// Computation of the ideal parameter matrix (theta):

[lAm,cAm]=size(Am)

[theta,theta_null]=mrcmatch(Nr,Dr,(poly(0,"s")*eye(lAm,cAm)-Am),A,Lambda)

// Computation of the plant high frequency gain matrix (Kp):

[lt,ct]=size(theta)

Kp=(inv(theta(lt-ct+1:lt,:)))'

K=Kp*Sp

// Solution of Lyapunov equation:

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

// Modulation function coefficients:

lmin = min(spec(Q))

lmin2 = 2/lmin

PKp = P * Kp

cd = (lmin2 * norm(P*K)) - 1

c1 = lmin2 * norm(PKp * (theta_nom-theta)')

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

c3 = lmin2 * norm(PKp)

endfunction