%
% Chapter 2, Example 2.4, and 2.5, pages 57-59
% Optimal Reference Shaping for Dynamical Systems
% Theory and Applications
% -Tarunraj Singh
%
%
% Flexible Transmission Control
%
clear all;
num = [0.10276 0.18123]
den = [1 -1.99185 2.20265 -1.84083 0.89413];
Ts = 1/20;  % 20 Hz sampling frequency

sysd = tf(num,den,Ts)

poles = roots(den);
plot(real(poles),imag(poles),'X');
hold on
th=linspace(0,2*pi,501);
x = cos(th); y = sin(th);
plot(x,y,'k');
clf

k = 20;
for ind = 0:k,
    temp = poles(1)^(-ind);
    A(1,ind+1) = real(temp);
    A(2,ind+1) = imag(temp);
    temp = poles(3)^(-ind);
    A(3,ind+1) = real(temp);
    A(4,ind+1) = imag(temp);
end;
A(5,:) = ones(1,size(A,2));
B = zeros(4,1);
B(5,1) = 1;
f = linspace(1,k+1,k+1); %ones(1,k+1);
f = f.*f.*f;
lb = zeros(1,k+1);
ub = ones(1,k+1);

xopt = linprog(f,[],[],A,B,lb,ub);
tvec = 0:Ts:(k+50)*Ts;
uref = ones(size(tvec));

AA = zeros(length(xopt));
for ind=1:length(AA),
    AA(ind,1:ind) = 1;
end;
xsum = AA*xopt;

uref(1:k+1) = xsum;
[y,t,x] = lsim(sysd,uref,tvec);
[yo,to] = step(sysd,3.5);
sysd1 = tf([0.28261 0.50666],[1 -1.41833 1.58939 -1.31608 0.88642],Ts); % No load case
sysd2 = tf([0.06408 0.10407],[1 -2.09679 2.31962 -1.93353 0.87129],Ts); % Full load case
[y1n,t1n,x1n] = lsim(sysd1,uref,tvec);
[y2n,t2n,x2n] = lsim(sysd2,uref,tvec);

figure(1)
stairs(t,y)
xlabel('Time (sec)'); ylabel('Output');
hold on
stairs(to,yo,'r--')

figure(2);
stairs(tvec,uref)
xlabel('Time (sec)'); ylabel('Shaped Command');
axis([0 3.5 0 1.2])

uref_n = uref;
% Design of Robust Time-Delay Filter for Discrete-time systems
%
k = 20;
for ind = 0:k,
    temp = poles(1)^(-ind);
    A(1,ind+1) = real(temp);
    A(2,ind+1) = imag(temp);
    temp = poles(3)^(-ind);
    A(3,ind+1) = real(temp);
    A(4,ind+1) = imag(temp);
    temp = -ind*poles(1)^(-ind-1);
    A(5,ind+1) = real(temp);
    A(6,ind+1) = imag(temp);
    temp = -ind*poles(3)^(-ind-1);
    A(7,ind+1) = real(temp);
    A(8,ind+1) = imag(temp);
end;
A(9,:) = ones(1,size(A,2));
B = zeros(8,1);
B(9,1) = 1;
f = linspace(1,k+1,k+1); %ones(1,k+1);
f = f.*f.*f;
lb = zeros(1,k+1);
ub = ones(1,k+1);

xopt = linprog(f,[],[],A,B,lb,ub);
tvec = 0:Ts:(k+50)*Ts;
uref = ones(size(tvec));

AA = zeros(length(xopt));
for ind=1:length(AA),
    AA(ind,1:ind) = 1;
end;
xsum = AA*xopt;

uref(1:k+1) = xsum;

[y,t,x] = lsim(sysd,uref,tvec);
[y1,t1,x1] = lsim(sysd1,uref,tvec);
[y2,t2,x2] = lsim(sysd2,uref,tvec);

figure(5);
stairs(t1n,y1n); hold on; stairs(t1,y1,'r--');
xlabel('Time (sec)'); ylabel('Output');

figure(6);
stairs(t2n,y2n); hold on; stairs(t2,y2,'r--');
xlabel('Time (sec)'); ylabel('Output');

figure(7);
stairs(tvec,uref,'r--'); hold on; stairs(tvec,uref_n);
xlabel('Time (sec)'); ylabel('Shaped Command');
