% This is a simple discrete-time adaptive controller

% Example 13.6

% Clear memory
clear all

% Define the input file names
exname = 'extdisc_ex6';    % the name of this macro

T = 0.01;    % sample period
kappa = 0.8; % discrete gain
c1 = 10;
c2 = 0.1;
phi = 0.1;

beta = T;
gamma = 1;
eta = 0.2;
k3 = 2 - eta/gamma;

k1 = 1 - kappa*kappa;
k2 = 0;
W = 2*c2;
rho = beta*beta*W + 0;

epsilon = 1e-6:1e-3:0.6;
bar_k1 = k1 + epsilon*k1 - epsilon;
bar_k2 = (1+epsilon)*k2 + 2*(1+1./epsilon)*beta*beta*(W + 2*rho/(beta*beta))^2;

% find the best epsilon
b_eps = sqrt(bar_k2./bar_k1);
[b_min, indx] = min(b_eps);
eps_min = epsilon(indx);
fprintf('b_min = %f for eps_min = %f\n', b_min, eps_min);

figure(1); clf
h1 = plot(epsilon, b_eps);
axis([0 .5 0 0.2]);
xlabel('\epsilon');
ylabel('b_{\epsilon}');
eval(sprintf('print -deps %s_a\n', exname));

bar_k1_min = k1 + eps_min*k1 - eps_min;
bar_k2_min = (1+eps_min)*k2 + 2*(1+1./eps_min)*beta*beta*(W + 2*rho/(beta*beta))^2;


bf = 1;
k3 = 2 - eta / gamma
k4 = 2*(1 + 1/eps_min)*T*T*(1 + gamma*bf*bf)/(eta*k3)
th = sqrt((9*9*bar_k1_min - bar_k2_min)/k4)

Vr = (bar_k2_min + k4*c1*c1)/bar_k1_min;
err = sqrt(Vr)


% Simulate the scaler case
maxits = 200;
r = ones(maxits+1, 1);
state = zeros(maxits, 2);
state(1,:) = [0];
xk1 = zeros(1,1);
k = 0:maxits;
k = k(:);
prev = 0;
for i=1:maxits

  % get the current states
  xk = state(i, 1);
  theta = state(i, 2);
  alpha = -r(i) + xk;

  % define the error variables
  err = xk(1) - r(i);

  % define the control input
  nu_s = (r(i+1) - xk(1) + kappa*err)/T;  % the static term
  nu_a = nu_s + theta;                  % the adaptive controller

  % advance the state
  ki = k(i);
  delta = c1 + c2*sin(2*pi*T*ki + phi);
  xk1(1) = xk(1) + T*(delta + nu_a);

  q = T*(err - prev);
  if (q > rho) bar_q = q - rho;
  elseif (q < -rho) bar_q = q + rho;
  else bar_q = 0;
  end;

  thetak1 = theta - eta*bar_q/(T*T*(1 + gamma));
  state(i+1, 1) = xk1';
  state(i+1, 2) = thetak1;

  % save static control term
  prev = alpha + beta*nu_s;
end
x_adapt = state;
theta

% Simulate the scaler case
maxits = 200;
r = ones(maxits+1, 1);
state = zeros(maxits, 2);
state(1,:) = [0];
xk1 = zeros(1,1);
k = 0:maxits;
k = k(:);
prev = 0;
for i=1:maxits

  % get the current states
  xk = state(i, 1);
  theta = state(i, 2);
  alpha = -r(i) + xk;

  % define the error variables
  err = xk(1) - r(i);

  % define the control input
  nu_s = (r(i+1) - xk(1) + kappa*err)/T;  % the static term
  nu_a = nu_s + theta;                  % the adaptive controller

  % advance the state
  ki = k(i);
  delta = c1 + c2*sin(2*pi*T*ki + phi);
  xk1(1) = xk(1) + T*(delta + nu_s);

  q = T*(err - prev);
  if (q > rho) bar_q = q - rho;
  elseif (q < -rho) bar_q = q + rho;
  else bar_q = 0;
  end;

  thetak1 = theta - eta*bar_q/(T*T*(1 + gamma));
  state(i+1, 1) = xk1';
  state(i+1, 2) = thetak1;

  % save static control term
  prev = alpha + beta*nu_s;
end
x_static = state;


figure(2); clf;
h1 = stairs(k, x_adapt(:,1)); hold on
h2 = stairs(k, x_static(:,1), '--'); hold on
xlabel('k');
axis([0, 150, 0, 1.6])
h3 = plot([0; 100], [1; 1], ':');
set([h1, h2, h3], 'LineWidth', 1.5);
xlabel('k');

eval(sprintf('print -deps %s_b\n', exname));