%
% Chapter 2, Example 2.1, pages 29-32
% Optimal Reference Shaping for Dynamical Systems
% Theory and Applications
% -Tarunraj Singh
%
% Example for PPD and PPMD controllers
%
close all
clear all

J = 10;
K1 = 2;
K2 = 2;

A = [0 1;-K1/J -K2/J];
B = [0;K1/J];
C = eye(2);
D = [0;0];
sys = ss(A,B,C,D);

omg = sqrt(K1/J);
zeta = K2/J/omg/2;
omgd = omg*sqrt(1-zeta^2);

T1 = pi/omgd;
T2 = 2*pi/omgd;
Tf = T2+2;
A1 = exp(zeta*pi/sqrt(1-zeta^2));
A1 = A1/(1+A1);

Tvec = [0 T1 Tf];
Avec = [A1 1-A1 0];

x0 = [0;0];
[y1,t1,x1] = vansim(sys,Avec,Tvec,x0,201);

figure(1)
clf
subplot(211), plot(t1,y1(:,1))
xlabel('Time (sec)')
ylabel('Displacement')
hold on
subplot(212), plot(t1,y1(:,2))
xlabel('Time (sec)')
ylabel('Velocity')
hold on

 
%
% Robust Control
%
A1 = exp(2*zeta*pi/sqrt(1-zeta^2));
A2 = 2*exp(zeta*pi/sqrt(1-zeta^2));
Norma = A1+A2+1;
A1 = A1/Norma;
A2 = A2/Norma;
Tvecr = [0 T1 T2 Tf];
Avecr = [A1 A2 1-A1-A2 0];

x0 = [0;0];
[y2,t2] = vansim(sys,Avecr,Tvecr,x0,201);

figure(1)
subplot(211), plot(t2,y2(:,1),'--r')
xlabel('Time (sec)')
ylabel('Displacement')
subplot(212), plot(t2,y2(:,2),'--r')
xlabel('Time (sec)')
ylabel('Velocity')

subplot(211), legend('PPD Filter','PPMD Filter')
subplot(212), legend('PPD Filter','PPMD Filter')

%
% Generate Sensitivity Plots
%
Jvec = linspace(0.7*J,1.3*J,51);
indJ = 1;
for J = Jvec,
    A = [0 1;-K1/J -K2/J];
    B = [0;K1/J];
    C = eye(2);
    D = [0;0];
    sys = ss(A,B,C,D);
    x0 = [0;0];
    [ysim,tsim] = vansim(sys,Avec,Tvec,x0);
    [ysimr,tsimr] = vansim(sys,Avecr,Tvecr,x0);
    Res_eng(indJ) = 0.5*ysim(end,2)^2*J + 0.5*(ysim(end,1)-1)^2*K1;
    Res_engr(indJ) = 0.5*ysimr(end,2)^2*J + 0.5*(ysimr(end,1)-1)^2*K1;
    indJ = indJ+1;
end;

figure(2)
plot(Jvec,sqrt(Res_eng),Jvec,sqrt(Res_engr),'--r');
xlabel('Uncertain Inertia');
ylabel('Square Root of Residual Energy')