function [u, e] = PID(s, u, e, Ki, Kp, Kd, deltat, s_target)
   u(2) = u(1);
   e(3) = e(2);
   e(2) = e(1);
   e(1) = s - s_target;
   u(1) = u(2) + Ki*e(1)*deltat + Kp*(e(1) - e(2)) + Kd*(e(1) - 2*e(2) + e(3))/deltat;
end