function pid_eval(Nd, kp, ki, kd)

% pid_eval(6, 0.002, 2.0e-6, 8.0)
% pid_eval(10, 0.0005, 1.0e-6, 4.0)

% Constants
kNoise = 0.05;
aNoise = 0.5;
alpha_i = 0.999

% Init
x = [zeros(1, 40), 2*ones(1, 160), zeros(1, 40)];
noise = kNoise*randn(length(x));

zfp = struct('velo', 0, 'dist', 0, 'x', 0);
zfc = []

ev = zeros(1, length(x));
efv = zeros(1, length(x));
y1v = zeros(1, length(x));
y2v = zeros(1, length(x));
ypv = zeros(1, length(x));
yiv = zeros(1, length(x));
ydv = zeros(1, length(x));

yvelo = 0;
ydist = 0;
state = 'Init';
y_last = 0;

b_f = [aNoise];
a_f = [1 -(1-aNoise)];
z_f = zeros(1, max(length(a_f), length(b_f))-1);

for n=1:length(x),
  e = x(n)-ydist + noise(n);
  [ef, z_f] = filter(b_f, a_f, e, z_f);
  
  [ypid, yc, zfc] = jpid(zfc, kp, ki, kd, alpha_i, ef);
  state_next = state;
  if strcmp(state, 'Init')
      state_next = 'Acquisition';
  elseif strcmp(state, 'Acquisition')
    if abs(ef) < 0.001
      kp = kp * 1.0;
      ki = ki * 1.0;
      kd = kd * 1.0;
      state_next = 'Track';
    end
  elseif strcmp(state, 'Track')
    if abs(ef) > 0.01
      kp = kp * 1.0;
      ki = ki * 1.0;
      kd = kd * 1.0;
      state_next = 'Acquisition';
    end
  end
  if strcmp(state_next, state) == 0
    disp(state);
  end
  dy = ypid - y_last;
  y_last = ypid;
  [yvelo, ydist, zfp] = plant(zfp, Nd, dy);
  ev(n) = e;
  efv(n) = ef;
  y1v(n) = ypid;
  y2v(n) = ydist;
  ypv(n) = zfc.yp;
  yiv(n) = zfc.yi;
  ydv(n) = zfc.yd;
  state = state_next;
end

if 1
subplot(3, 1, 1)
plot(1:length(x), ev, 1:length(x), efv); grid; legend("Error", "Error_{Filtered}")
subplot(3, 1, 2)
plot(1:length(x), y1v, 1:length(x), ypv, 1:length(x), yiv, 1:length(x), ydv); grid; legend("PID", "P", "I", "D")
subplot(3, 1, 3)
plot(1:length(x), x, 1:length(x), y2v); grid; legend("Target", "Plant")
end

function [yvelo, ydist, zf] = plant(zi, nd, x)
b = [zeros(1, nd-1) 0.01];
a = [1 -1];
if zi.velo == 0
  zi.velo = zeros(1, max(length(a), length(b))-1);
end
if zi.dist == 0
  zi.dist = zeros(1, 1);
end

[yvelo, zf.velo] = filter(b, a, x, zi.velo);
[ydist, zf.dist] = filter([1], [1 -1], yvelo, zi.dist);