% Double integrator+delay example
% with hp=K*exp(-tau*s)/s^2

K=1; tau=1;

s=tf('s');

hp=K*(1-tau*s)/(s^2+0*0.001);   % Approx delay as inverse response

Td=1/(2*K*tau);       % Derivative time constant
hc=Td*s+0*0.1;          % Use PD control with model uncertainty

h0=hc*hp;             % Loop transfer function

[Gm,Pm,Wcg,Wcp] = margin(h0)

% Tranfer function of feedback system: y=h_ry*r
h_ry=h0/(1+h0);

step(h_ry)      % Step response

% Test PD controller
Kp=4/(K*Td^2)
hc2=Kp*(1+Td*s);          % Use PD control with model uncertainty

h02=hc2*hp;
h_ry2=h02/(1+h02);

figure(2)
step(h_ry2)