%
% Chapter 2, Example 2.3, pages 52-54
% Optimal Reference Shaping for Dynamical Systems
% Theory and Applications
% -Tarunraj Singh
%
% Example for PPD and PPMD controllers
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Simulate system with high frequency unmodelled dynamics
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1); clf
figure(2); clf
Jmax = 4;
m = 1;
ms = 0.1;
M = [(m+ms) ms;ms ms];
k = 1;
ks = 50;
K = [k 0;0 ks];

A = [zeros(2) eye(2);-inv(M)*K zeros(2)];
B = [0;0;inv(M)*[1;0]];
C = [1 0 0 0;0 1 0 0];
D = [0;0];

sysF = ss(A,B,C,D);
%%%%%%%%%%%%%%%%%%%%%%% STEP RESPONSE OF SYSTEM%%%%%%%%%%%%%%%%%%%%
tvec = linspace(0,10,5001);
Stepresp = step(sysF,tvec);
figure(1)
subplot(211), plot(tvec,Stepresp(:,1))
xlabel('Time')
ylabel('Displacement')
subplot(212), plot(tvec,Stepresp(:,2))
xlabel('Time')
ylabel('Displacement (Unmodelled Mode)')


ev = eig(A);

om1 = min(abs(imag(ev)));
om = max(abs(imag(ev)));    % Second Frequency of system
om = Jmax*2;                % Frequency of the harmonic shaper to match the jerk constrait.

t = linspace(0,2*pi*(1/om+1/om1),20001);

T1 = pi/om;
T2 = pi/om1;
T3 = pi*(1/om+1/om1);
T4 = t(end);

u = zeros(size(t));
indvec = find(t < T1);
u(indvec) = 0.25;

indvec = find(t < T2 & t >= T1);
u(indvec) = 0.5;

indvec = find(t < T3 & t >= T2);
u(indvec) = 0.75;

indvec = find(t <= T4 & t >= T3);
u(indvec) = 1.0;

num = [om*om]; den = [1 0 om*om];
sys = tf(num,den);

ufilt2 = lsim(sys,u,t);

%
% Finite Jerk Input Shaper
%
T1 = 1/2/Jmax;
T2 = pi/om1;
T3 = pi/om1+1/2/Jmax;

u = zeros(size(t));
indvec = find(t < T1);
u(indvec) = 1;

indvec = find(t < T2 & t >= T1);
u(indvec) = 0.0;

indvec = find(t < T3 & t >= T2);
u(indvec) = 1.0;

indvec = find(t <= T4 & t >= T3);
u(indvec) = 0.0;

sys = tf([Jmax],[1 0])

ufilt1 = lsim(sys,u,t);

%
% Finite Jerk First Order Filtered Input Shaper
%
a = 10;
T = 1/a*log(2*Jmax/(2*Jmax-a)); % Use 2*J since the time-delay filter for the undamped system scales the jerk by 1/2
T1 = pi/om1;
T2 = T1+T;
T3 = t(end);

u = zeros(size(t));
indvec = find(t < T);
u(indvec) = -exp(a*T)/(1-exp(a*T))*0.5;

indvec = find(t < T1 & t >= T);
u(indvec) = 0.5;

indvec = find(t < T2 & t >= T1);
u(indvec) = 0.5-exp(a*T)/(1-exp(a*T))*0.5;

indvec = find(t < T3 & t >= T2);
u(indvec) = 1.0;

sys = tf([a],[1 a])

ufilt3 = lsim(sys,u,t);


% Generate output of the System
T1 = pi/om1;
T2 = t(end);

uunfilt = zeros(size(t));
indvec = find(t < T1);
uunfilt(indvec) = 0.5;

indvec = find(t <= T2 & t >= T1);
uunfilt(indvec) = 1.0;

[yu,tu] = lsim(sysF,uunfilt,t);
figure(2)
subplot(211), plot(tu,yu(:,1))
hold on
subplot(212), plot(tu,yu(:,2))
hold on

[y1,t1] = lsim(sysF,ufilt1,t);
subplot(211), plot(t1,y1(:,1),'k')
subplot(212), plot(t1,y1(:,2),'k')

[y2,t2] = lsim(sysF,ufilt2,t);
subplot(211), plot(t2,y2(:,1),'r')
subplot(212), plot(t2,y2(:,2),'r')

[y3,t3] = lsim(sysF,ufilt3,t);
subplot(211), plot(t3,y3(:,1),'c')
xlabel('Time')
ylabel('Displacement')
subplot(212), plot(t3,y3(:,2),'c')
xlabel('Time')
ylabel('Displacement (Unmodelled Mode)')
legend('Input Shaper','Jerk Limited Input Shaper','Sinusoid Filtered Input Shaper','First Order Filtered Input Shaper');

figure(3)
plot(t,uunfilt,t,ufilt1,'k',t,ufilt2,'r',t,ufilt3,'c');
xlabel('Time')
ylabel('Control')
legend('Input Shaper','Jerk Limited Input Shaper','Sinusoid Filtered Input Shaper','First Order Filtered Input Shaper');


