//---------------------------------------------------------------------------
//
//   Design of first order nonlinear filters for the approximation of
//   the norm of the output signal of a linear filter:
//   graphics of the cost function S(gamma).
//
//   Developed for Scilab 2.6.
//
//   Rio de Janeiro, May 27, 2002.
//
//   Author: Jose' Paulo V. S. da Cunha
//
//---------------------------------------------------------------------------

// Linear filter (G(s)) realization:

//A = [0 1 0 0 0; -100.25 -1 1 0 0; 0 0 -1 1 0; 0 0 0 -1 1; 0 0 0 0 -1]
//
//B = [0; 5; 0; 0; 2]
//
//C = [3 0 10 0 0]

A = [  0      1        0       0     0;
       0      0        1       0     0;
       0      0        0       1     0;
       0      0        0       0     1;
    -100.25 -301.75 -304.75 -106.25 -4 ]

B = [0;
     0;
     0;
     0;
     1]

C = [119 3 5 1 0]

// Design parameters:

delta_u = 0.001;

delta_l = 0.05;

admissible_error = 0.001;

horizon = 10;

// Characteristics of G(s)=C(sI-A)^-1B:

G=syslin('c',A,B,C);

disp('Transfer function G(s):');

ss2tf(G)

disp('Zeros of G(s):');

trzeros(G)

disp('Poles of G(s):');

// Eigenvalues of the filter realization:

eigenvalues = spec(A)

// Stability margin:

gamma_0 = -max(real(eigenvalues));

// Step size and time range selection for filter impulse response simulation:

abs_eigenvalues = abs(eigenvalues);

//h = admissible_error/max(abs_eigenvalues);

h = 0.01;

//Range = int(0.5+horizon/(min(abs_eigenvalues)*h));

Range = 400;

// Bounds for the nonlinear filter parameters:

c_min = norm(C*B);

gamma_max = gamma_0 - delta_u;

// Impulse response computation:

PHI = expm(A*h);

X = B;

t = zeros(Range+1);

g_norm = zeros(Range+1);

g_norm(1) = c_min;

g_norm_max = c_min;

k_g_norm_max = 1;

exp_gamma_h = exp(-gamma_max*h);

exp_gamma_k = 1;

relation_max = c_min;

k_relation_max = 1;

k = 1;

while k<=Range do
   k1 = k+1;
   X = PHI*X;
   t(k1) = h*k;
   g_norm(k1) = norm(C*X);
   exp_gamma_k = exp_gamma_k*exp_gamma_h;
   relation = g_norm(k1)/exp_gamma_k;
// Find the peak value of the relation (norm/exp):
   if relation>=relation_max then
     relation_max = relation;
     k_relation_max = k1;
   end;
// Find the peak value of the norm of the impulse response:
   if g_norm(k1)>=g_norm_max then
     g_norm_max = g_norm(k1);
     k_g_norm_max = k1;
   end;
   k = k1;
end;

c = relation_max;

c_gamma_max = relation_max;

Gamma = gamma_max;

S_graphics = c/Gamma;

Gamma_graphics = Gamma;

j = 1;

j_final = int(1/admissible_error);

decreasing = %T;

while ((j<=j_final) & decreasing) do
   possible_gamma = gamma_max-(gamma_max-delta_l)*(admissible_error*j);
   exp_gamma_h = exp(-possible_gamma*h);
   exp_gamma_k = exp(-possible_gamma*(k_g_norm_max-1)*h);
   relation_max = c_min;
   k_relation_max1 = k_relation_max;
   k = k_g_norm_max;
   while k<=k_relation_max do
     k1 = k+1;
     exp_gamma_k = exp_gamma_k*exp_gamma_h;
     relation = g_norm(k1)/exp_gamma_k;
// Find the peak value of the relation (norm/exp):
     if relation>=relation_max then
       relation_max = relation;
       k_relation_max1 = k1;
     end;
     k = k1;
   end;
   k_relation_max = k_relation_max1;
   if (c/Gamma)>(relation_max/possible_gamma) then
     c = relation_max;
     Gamma = possible_gamma;
//   else
//     if (c/Gamma)<(relation_max/possible_gamma) then
//       decreasing = %F;
//     end;
   end;

S_graphics = [(relation_max/possible_gamma) S_graphics];

Gamma_graphics = [possible_gamma Gamma_graphics];

   j = j+1;
end;

// Print and plot the results:

c_gamma_max

gamma_max

c_gamma_max/gamma_max

c

Gamma

c/Gamma

//xbasc();

//plot2d(t,[g_norm (c*exp(-Gamma*t)) (c_gamma_max*exp(-gamma_max*t))]);


disp('Displaying the gamma versus DC gain graphics...');
xbasc();
xset("default");
xset("font size",4);
xset("line style",1);
//plot2d(Gamma_graphics,S_graphics,rect=[0,0,0.5,20]);
plot2d(Gamma_graphics,S_graphics);
xtitle('','gamma (1/s)','S');

disp('Type RETURN to show the derivative of the DC gain with respect to gamma...');
pause;

// Computing the derivative of S with respect to gamma:

dS=convol(S_graphics,[1,-1])/((gamma_max-delta_l)*admissible_error);

dS=dS(1:(j_final+1));

xbasc();
xset("default");
xset("font size",4);
xset("line style",1);
plot2d(Gamma_graphics,dS,rect=[0.1,-40,0.5,10]);
xset("line style",2);
xgrid(1);
xtitle('','gamma (1/s)','dS/d gamma (s)');

disp('Type RETURN to show the norm of the impulse response...');
pause;
xbasc();
xset("default");
xset("font size",4);
xset("line style",1);
plot2d(t,g_norm);
xtitle('','t (s)','Impulse response norm');

// Search of the trust region:

j = Range;

trust_region_elements =1;

great_g_norm = g_norm(Range+1);

while j>=1 do
  if great_g_norm<g_norm(j) then
    trust_region_elements = trust_region_elements+1;
    great_g_norm = g_norm(j);
  end;
  j = j-1;
end;

disp('Number of elements which belong to the trust region:');

total_elements = Range + 1;

total_elements

trust_region_elements

h

disp('Type RETURN to show the norm of the impulse response and FOAF impulse response...');
pause;
xbasc();
xset("default");
xset("font size",4);
xset("line style",1);
plot2d(t,[g_norm, c*exp(-Gamma*t), c_gamma_max*exp(-gamma_max*t)]);
xtitle('','t (s)','Impulse response norm');