% Polynomial Design of Double Integrator
% 

ac = 0.9; % closed loop poles (2)
a0 = 0.8; % observer pole (1)

% Linear equations system RA+SB=AcA0
Bezoutmatrix=[1 1 0 ; -2 1 1 ; 1 0 1 ];
RHS = [2-2*ac-a0; -1+ac^2+2*a0*ac; -ac^2*a0];
RS = Bezoutmatrix\RHS;
R=[1 RS(1)];
S=[RS(2) RS(3)];
% Unit DC gain
k=(1-ac)^2/2;
T=k*A0;

% Check result
RASB = conv(R,A)+ conv(S,B);
closedlooppoles = roots(RASB)
DCgain = polyval(B,1)*polyval(T,1)/polyval(RASB,1)

% Simulate step response
B=[0 1 1];
A=[1 -2 1];
tfinal=100;

figure(1);
clsys=tf(conv(B,T),RASB,-1);
[y,t]=step(clsys,tfinal);
subplot(211)
stairs(t,y,'b','linewidth',1.6)
set(gca,'fontsize',16,'fontweight','b')
title('y')
hold on
subplot(212)
[u,t]=step(tf(conv(A,T),RASB,-1),tfinal);
stairs(t,u,'b','linewidth',1.6)
set(gca,'fontsize',16,'fontweight','b')
title('u')
hold on

% % Second design (not so good, cancelling B)
% a0=-1;  % Cancel B-polynomial
% RHS = [2-2*ac-a0; -1+ac^2+2*a0*ac; -ac^2*a0];
% RS = Bezoutmatrix\RHS;
% r1=RS(1);
% s0=RS(2);
% s1=RS(3);
% R=[1 r1];
% S=[s0 s1];
% k=(1-ac)^2;
% T=k*[0 1];
% 
% RASB = conv(R,A)+ conv(S,B);
% % Check result
% RASB = conv(R,A)+ conv(S,B);
% closedlooppoles = roots(RASB)
% DCgain = polyval(B,1)*polyval(T,1)/polyval(RASB,1)
% 
% figure(1);
% clsys=tf(conv(B,T),RASB,-1);
% step(clsys,tfinal);
% figure(2)
% step(tf(conv(A,T),RASB,-1),tfinal);


 
