//---------------------------------------------------------------------------
//
//   Synthesis of a first order nonlinear filter for approximation of
//   the norm of the output signal of a linear multivariable filter (FOAF).
//
//   Developed for Scilab 2.6.
//
//   Rio de Janeiro, May 13, 2002.
//
//   Author: Jose' Paulo V. S. da Cunha
//
//   Last review: June 04, 2002.
//
//---------------------------------------------------------------------------

//-------------------------------------------------------------------------------
//
// Function to the synthesis of a first order nonlinear filter for approximation
// of the norm of the output signal of a linear multivariable filter. The method
// applies optimization with three distinct objectives: (i) minimize the filter
// DC gain, (ii) minimize the filter time constant and (iii) minimize the peak
// of the impulse response of the filter. This function also returns the
// Euclidian norm of the original filter impulse response.
//
//   Rio de Janeiro, May 13, 2002.
//
//   Author: Jose' Paulo V. S. da Cunha
//
//   Last review: June 04, 2002.
//
//-------------------------------------------------------------------------------
//
// Variables:
// 
// A,B,C                 - Matrices of the linear filter realization
//                         (G(s) = C(sI-A)^-1 B).
// delta_u               - Design parameter that defines the upperbound
//                         for the feasible Gamma (gamma_max = gamma_0 - delta_u).
// delta_l               - Lower bound for the feasible Gamma.
// epsilon1              - Parameter in the range (0,pi). The error becomes
//                         larger as 'epsilon1' grows. Recommended value: 0.1.
// epsilon2              - Maximum allowed relative error on Gamma.
// ho                    - Impulse simulation horizon tf = ho/min(|spec(A)|).
//                         Recommended value: 10.
// c, Gamma              - Coefficients for the minimum DC gain filter
//                         (c/(s+Gamma)).
// c_gamma_max,gamma_max - Coefficients for the minimum time constant filter
//                         (c_gamma_max/(s+gamma_max)).
// c_delta_l             - Coefficient for the filter with minimum peak value
//                         of the impulse response (c_delta_l/(s+delta_l)).
// g_norm                - Linear filter impulse response norm
//                         (||g(t)||=||Cexp(At)B||).
// t                     - Time vector corresponding to g_norm.
//
// Notes:
//
// 1) ||g(t) * u(t)|| <= c exp(-Gamma t) * ||u(t)||.
//
// 2) gamma_0 is the stability margin of matrix A.
//
// 3) delta_l<= Gamma <= gamma_max,  gamma_max = gamma_0 - delta_u.
//
//-------------------------------------------------------------------------------

function [c,Gamma,c_gamma_max,gamma_max,c_delta_l,t,g_norm]=first_order_filter(A,B,C,delta_u,delta_l,epsilon1,epsilon2,ho)

//
// Stops if some parameter is out of range:
//
if (delta_u<=0) | (delta_l<=0) | (epsilon1<=0) | (epsilon1>=%pi) | (epsilon2<=0) | (ho<=0) then
   error('Cannot design FOAF: some input parameter out of range.');
end;

//
// Eigenvalues of the filter realization:
//
eigenvalues = spec(A);

//
// Stability margin:
//
gamma_0 = -max(real(eigenvalues));

//
// Stops if the filter is unstable:
//
if gamma_0 <=0 then
   error('Cannot design FOAF: unstable filter.');
end;

//
// Step size (h) and time range (kf) selection for filter impulse
// response simulation:
//
abs_eigenvalues = abs(eigenvalues);

h = epsilon1/max(abs_eigenvalues);

kf = int(0.5+ho/(min(abs_eigenvalues)*h));

kf1 = kf+1;

//
// Upperbound for gamma:
//
gamma_max = gamma_0 - delta_u;

//
// Stops if the design parameters delta_u and delta_l are too large:
//
if gamma_max < delta_l then
   error('Cannot design FOAF: (delta_u+delta_l) is too large.');
end;

//
// Impulse response computation:
//
PHI = expm(A*h);

X = B;

t = zeros(kf1);

g_norm = zeros(kf1);

g_norm(1) = norm(C*B,2);

k = 1;

while k<=kf do
   k1 = k+1;
   X = PHI*X;
   t(k1) = h*k;
   g_norm(k1) = norm(C*X,2);
   k = k1;
end;

// Find the thrust region (T):

T = zeros(kf1);

T(1) = kf1;

kbegin = 1;

g_norm_peak = g_norm(kf1);

k = kf;

while k>=1 do
  if g_norm(k)>g_norm_peak then
    kbegin = kbegin+1;
    T(kbegin) = k;
    g_norm_peak = g_norm(k);
  end;
  k = k-1;
end;

//
// Find c_gamma_max = C1(gamma_max):
//
gamma_max_h = gamma_max*h;

exp_gamma_h = exp(-gamma_max_h);

exp_gamma_k = exp(-gamma_max_h*(T(kbegin)-1));

C1 = g_norm(T(kbegin))/exp_gamma_k;

k = kbegin;

kfinal = 1;

while k>1 do
   k1 = k-1;
   if T(k1)==(T(k)+1) then
     exp_gamma_k = exp_gamma_k*exp_gamma_h;
   else
     exp_gamma_k = exp(-gamma_max_h*(T(k1)-1));
   end;
   relation = g_norm(T(k1))/exp_gamma_k;
//
// Find the peak value of the relation (norm/exp) and the first sample
// of its occurence:
//
   if relation>C1 then
     C1 = relation;
     kfinal = k1;
   end;
   k = k1;
end;

c_gamma_max = C1;

//
// Find c_delta_l = C1(delta_l):
//
delta_l_h = delta_l*h;

exp_gamma_h = exp(-delta_l_h);

exp_gamma_k = exp(-delta_l_h*(T(kbegin)-1));

C1 = g_norm(T(kbegin))/exp_gamma_k;

k = kbegin;

while k>kfinal do
   k1 = k-1;
   if T(k1)==(T(k)+1) then
     exp_gamma_k = exp_gamma_k*exp_gamma_h;
   else
     exp_gamma_k = exp(-delta_l_h*(T(k1)-1));
   end;
   relation = g_norm(T(k1))/exp_gamma_k;
//
// Find the peak value of the relation (norm/exp) and the last sample
// of its occurence:
//
   if relation>=C1 then
     C1 = relation;
     kbegin = k1;
   end;
   k = k1;
end;

c_delta_l = C1;

//
// Computation of c and Gamma through Golden Search:
//

// Golden ratio (r=(3-sqrt(5))/2, r1=1-r):
//
r = 0.3819660;

r1 = 0.6180340;

// Initial points:
//
gamma1 = delta_l;

gamma4 = gamma_max;

gamma2 = gamma1*r1+gamma4*r;

gamma3 = gamma1*r+gamma4*r1;

// Peak values and corresponding areas:

c_gamma1 = c_delta_l;

c_gamma4 = c_gamma_max;

S1 = c_gamma1/gamma1;

S4 = c_gamma4/gamma4;

//
// Find c_gamma2 = C1(gamma2):
//
gamma2_h = gamma2*h;

exp_gamma_h = exp(-gamma2_h);

exp_gamma_k = exp(-gamma2_h*(T(kbegin)-1));

C1 = g_norm(T(kbegin))/exp_gamma_k;

k = kbegin;

k2 = kbegin;

while k>kfinal do
   k1 = k-1;
   if T(k1)==(T(k)+1) then
     exp_gamma_k = exp_gamma_k*exp_gamma_h;
   else
     exp_gamma_k = exp(-gamma2_h*(T(k1)-1));
   end;
   relation = g_norm(T(k1))/exp_gamma_k;
//
// Find the peak value of the relation (norm/exp) and the last sample
// of its occurence:
//
   if relation>=C1 then
     C1 = relation;
     k2 = k1;
   end;
   k = k1;
end;

c_gamma2 = C1;

S2 = c_gamma2/gamma2;

//
// Find c_gamma3 = C1(gamma3):
//

gamma3_h = gamma3*h;

exp_gamma_h = exp(-gamma3_h);

exp_gamma_k = exp(-gamma3_h*(T(k2)-1));

C1 = g_norm(T(k2))/exp_gamma_k;

k = k2;

k3 = kfinal;

while k>kfinal do
   k1 = k-1;
   if T(k1)==(T(k)+1) then
     exp_gamma_k = exp_gamma_k*exp_gamma_h;
   else
     exp_gamma_k = exp(-gamma3_h*(T(k1)-1));
   end;
   relation = g_norm(T(k1))/exp_gamma_k;
//
// Find the peak value of the relation (norm/exp) and the first sample
// of its occurence:
//
   if relation>C1 then
     C1 = relation;
     k3 = k1;
   end;
   k = k1;
end;

c_gamma3 = C1;

S3 = c_gamma3/gamma3;

//
// Golden search loop:
//
while (gamma3-gamma1)>(epsilon2*gamma1) do
  if S2>=S3 then
    gamma1 = gamma2;
    gamma2 = gamma3;
    gamma3 = gamma1*r+gamma4*r1;
    c_gamma1 = c_gamma2;
    c_gamma2 = c_gamma3;
    S1 = S2;
    S2 = S3;
    kbegin = k2;
//
// Find c_gamma3 = C1(gamma3):
//
    gamma3_h = gamma3*h;
    exp_gamma_h = exp(-gamma3_h);
    exp_gamma_k = exp(-gamma3_h*(T(kbegin)-1));
    C1 = g_norm(T(kbegin))/exp_gamma_k;
    k = kbegin;
    k3 = kfinal;
    while k>kfinal do
       k1 = k-1;
       if T(k1)==(T(k)+1) then
         exp_gamma_k = exp_gamma_k*exp_gamma_h;
       else
         exp_gamma_k = exp(-gamma3_h*(T(k1)-1));
       end;
       relation = g_norm(T(k1))/exp_gamma_k;
//
// Find the peak value of the relation (norm/exp) and the first sample
// of its occurence:
//
       if relation>C1 then
         C1 = relation;
         k3 = k1;
       end;
       k = k1;
    end;
    c_gamma3 = C1;
    S3 = c_gamma3/gamma3;
  else
    gamma4 = gamma3;
    gamma3 = gamma2;
    gamma2 = gamma1*r1+gamma4*r;
    c_gamma4 = c_gamma3;
    c_gamma3 = c_gamma2;
    S4 = S3;
    S3 = S2;
    kfinal = k3;
//
// Find c_gamma2 = C1(gamma2):
//
    gamma2_h = gamma2*h;
    exp_gamma_h = exp(-gamma2_h);
    exp_gamma_k = exp(-gamma2_h*(T(kbegin)-1));
    C1 = g_norm(T(kbegin))/exp_gamma_k;
    k = kbegin;
    k2 = kbegin;
    while k>kfinal do
       k1 = k-1;
       if T(k1)==(T(k)+1) then
         exp_gamma_k = exp_gamma_k*exp_gamma_h;
       else
         exp_gamma_k = exp(-gamma2_h*(T(k1)-1));
       end;
       relation = g_norm(T(k1))/exp_gamma_k;
//
// Find the peak value of the relation (norm/exp) and the last sample
// of its occurence:
//
       if relation>=C1 then
         C1 = relation;
         k2 = k1;
       end;
       k = k1;
    end;
    c_gamma2 = C1;
    S2 = c_gamma2/gamma2;
  end;
end;

// Selects the sub-optimum with minimum DC gain (S):

S = S4;
c = c_gamma4;
Gamma = gamma4;

if S3 < S then
  S = S3;
  c = c_gamma3;
  Gamma = gamma3;
end;

if S2 < S then
  S = S2;
  c = c_gamma2;
  Gamma = gamma2;
end;

if S1 < S then
  c = c_gamma1;
  Gamma = gamma1;
end;

endfunction

//-------------------------------------------------------------------------------
//
// Function to the synthesis of a first order nonlinear filter for approximation
// of the norm of the output signal of a linear multivariable filter. The method
// applies Lyapunov synthesis and may give conservative results.
//
//   Rio de Janeiro, May 14, 2002.
//
//   Author: Jose' Paulo V. S. da Cunha
//
//   Last review: May 14, 2002.
//
//-------------------------------------------------------------------------------
//
// Variables:
// 
// A,B,C                 - Matrices of the linear filter realization
//                         (G(s)=C(sI-A)^-1 B).
// c1, Gamma             - Coefficients for the filter (c1/(s+Gamma)).
// c2_                   - Coefficient for the transient term of the filter.
//
// Note: ||y(t)|| <= c1 exp(-Gamma t) * ||u(t)|| + c2 exp(-Gamma t) ||x(0)||.
//
//-------------------------------------------------------------------------------

function [c1,c2,Gamma]=first_order_lyapunov_filter(A,B,C)

// Solve the Lyapunov equation A'P+PA=-2I:

n = size(A,'c');

P = lyap(A,-2*eye(n,n),'c');

// Compute eigenvalues of P:

eigenvalues = spec(P);

lambda_min = min(eigenvalues);

lambda_max = max(eigenvalues);

// Compute the first order approximation filter pole:

Gamma = 1/lambda_max;

// Compute the first order approximation filter coefficient:

C_norm = norm(C,2);

c1 = C_norm*norm(P*B,2)/lambda_min;

// Compute the transient term coefficient:

c2 = C_norm*sqrt(lambda_max/lambda_min);

endfunction