%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% help ltimodels
% help ss

T = 0.01

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Pure mass (double integrator) continuous time
A = [ 0 1 
      0 0 ]
B = [ 0
      1 ]
C = [ 1 0 ]
s = ss(A,B,C,0);

bode(s)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Now control it.
nat_freq = 5; %Hz
nat_freq_rad = 2*pi*nat_freq;
pos_gain = nat_freq_rad*nat_freq_rad; % w^2 = k/m
vel_gain = nat_freq_rad*2; % damp_ratio = b/(2*sqrt(km))
K = [pos_gain vel_gain];
sc = ss(A-B*K,B,C,0);
pole(sc)
zero(sc)
damp(sc)
pzmap(sc)
bode(sc)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Simulate response to non-zero goal.
x = [ 0 0 ]'
xd = [ 1 0 ]'

nsteps = 100;
data(nsteps,2) = 0;
data2(nsteps,1) = 0;

for i = 1:nsteps
data(i,:) = x;
u = -K*(x-xd);
data2(i,:) = u;
x = x + (A*x + B*u)*T;
end

plot(data(:,1))
plot(data(:,2))

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% root locus
pzmap(sc)
hold
for i = 1:1:10
 nat_freq = i; %Hz
 nat_freq_rad = 2*pi*nat_freq;
 pos_gain = nat_freq_rad*nat_freq_rad;
 vel_gain = nat_freq_rad*2;
 K = [pos_gain vel_gain];
 sc = ss(A-B*K,B,C,0);
 pole(sc)
 pzmap(sc)
end
hold

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Pure mass (double integrator) discrete time
A1 = [ 1 T 
      0 1 ]
B1 = [ T*T/2
       T ]
C1 = [ 1 0 ]

s1 = ss(A1,B1,C1,0,T);

bode(s1)

bode(s,'b',s1,'r')

% Now control it.
nat_freq = 5; %Hz
nat_freq_rad = 2*pi*nat_freq;
pos_gain = nat_freq_rad*nat_freq_rad;
vel_gain = nat_freq_rad*2;
K = [pos_gain vel_gain];
s1c = ss(A1-B1*K,B1,C1,0,T);
pole(s1c)
zero(s1c)
damp(s1c)
pzmap(s1c)
bode(s1c)

bode(sc,'b',s1c,'r')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Goes unstable at 16Hz.

% root locus
pzmap(s1c)
hold
for i = 1:1:17
 nat_freq = i; %Hz
 nat_freq_rad = 2*pi*nat_freq;
 pos_gain = nat_freq_rad*nat_freq_rad;
 vel_gain = nat_freq_rad*2;
 K = [pos_gain vel_gain];
 s1c = ss(A1-B1*K,B1,C1,0,T);
 abs(pole(s1c))
 pzmap(s1c)
end
hold

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Add delay 10ms
A2 = [ 1 T T*T/2
       0 1 T
       0 0 0 ]
B2 = [ 0
      0
      1 ]
C2 = [ 1 0 0 ]

s2 = ss(A2,B2,C2,0,T);

bode(s2)

bode(s,'b',s1,'g',s2,'r')

% Now control it.
nat_freq = 5; %Hz
nat_freq_rad = 2*pi*nat_freq;
pos_gain = nat_freq_rad*nat_freq_rad;
vel_gain = nat_freq_rad*2;
K = [pos_gain vel_gain 0];
s2c = ss(A2-B2*K,B2,C2,0,T);
pole(s2c)
abs(pole(s2c))

bode(s2c)

bode(sc,'b',s1c,'g',s2c,'r')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Goes unstable at 7Hz.

% root locus
pzmap(s2c)
hold
for i = 1:1:8
 nat_freq = i; %Hz
 nat_freq_rad = 2*pi*nat_freq;
 pos_gain = nat_freq_rad*nat_freq_rad;
 vel_gain = nat_freq_rad*2;
 K = [pos_gain vel_gain 0];
 s2c = ss(A2-B2*K,B2,C2,0,T);
 abs(pole(s2c))
 pzmap(s2c)
end
hold

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Add delay 100ms (human-like)
A3 = [ 1 T T*T/2 0 0 0 0 0 0 0 0 0
       0 1 T 0 0 0 0 0 0 0 0 0 
       0 0 0 1 0 0 0 0 0 0 0 0
       0 0 0 0 1 0 0 0 0 0 0 0
       0 0 0 0 0 1 0 0 0 0 0 0
       0 0 0 0 0 0 1 0 0 0 0 0
       0 0 0 0 0 0 0 1 0 0 0 0
       0 0 0 0 0 0 0 0 1 0 0 0
       0 0 0 0 0 0 0 0 0 1 0 0
       0 0 0 0 0 0 0 0 0 0 1 0
       0 0 0 0 0 0 0 0 0 0 0 1
       0 0 0 0 0 0 0 0 0 0 0 0 ]
B3 = [ 0
      0
      0
      0
      0
      0
      0
      0
      0
      0
      0
      1 ]
C3 = [ 1 0 0 0 0 0 0 0 0 0 0 0 ]

s3 = ss(A3,B3,C3,0,T);

bode(s3)

bode(s,'b',s1,'g',s2,'m',s3,'r')

% Now control it.
nat_freq = 0.5; %Hz
nat_freq_rad = 2*pi*nat_freq;
pos_gain = nat_freq_rad*nat_freq_rad;
vel_gain = nat_freq_rad*2;
K = [pos_gain vel_gain 0 0 0 0 0 0 0 0 0 0];
s3c = ss(A3-B3*K,B3,C3,0,T);
pole(s3c)
abs(pole(s3c))

bode(s3c)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Goes unstable at 1.0

% root locus
pzmap(s3c)
hold
for i = 0.1:0.1:1.1
 nat_freq = i; %Hz
 nat_freq_rad = 2*pi*nat_freq;
 pos_gain = nat_freq_rad*nat_freq_rad;
 vel_gain = nat_freq_rad*2;
 K = [pos_gain vel_gain 0 0 0 0 0 0 0 0 0 0];
 s3c = ss(A3-B3*K,B3,C3,0,T);
 abs(pole(s3c))
 pzmap(s3c)
end
hold

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Add collocated flexibility
% state: pos, vel, flex-pos, flex-vel
% continuous time
KK = 10.0
BB= 0.01
A4 = [ 0 1 0 0
       0 0 2*KK 2*BB
       0 0 0 1
       0 0 -4*KK -4*BB ]
B4 = [ 0
2
0
-2 ]
C4 = [ 1 0 0 0 ]

s4 = ss(A4,B4,C4,0);

bode(s4)

bode(s,'b',s1,'g',s4,'r')

% Now control it.
nat_freq = 5; %Hz
nat_freq_rad = 2*pi*nat_freq;
pos_gain = nat_freq_rad*nat_freq_rad;
vel_gain = nat_freq_rad*2;
K = [pos_gain vel_gain 0 0];
s4c = ss(A4-B4*K,B4,C4,0);
pole(s4c)

bode(s4c)

bode(sc,'b',s1c,'g',s4c,'r')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% root locus
pzmap(s4c)
hold
for i = 1:1:10
 nat_freq = i; %Hz
 nat_freq_rad = 2*pi*nat_freq;
 pos_gain = nat_freq_rad*nat_freq_rad;
 vel_gain = nat_freq_rad*2;
 K = [pos_gain vel_gain 0 0];
 s4c = ss(A4-B4*K,B4,C4,0);
 pole(s4c)
 pzmap(s4c)
end
hold

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Add non-collocated flexibility
% state: pos, vel, flex-pos, flex-vel
% continuous time
KK = 10.0
BB = 1.0
A5 = [ 0 1 0 0
       0 0 2*KK 2*BB
       0 0 0 1
       0 0 -4*KK -4*BB ]
B5 = [ 0
2
0
-2 ]
C5 = [ 1 0 1 0 ]

s5 = ss(A5,B5,C5,0);

bode(s5)

bode(s,'b',s1,'g',s4,'c',s5,'r')

% Now control it.
nat_freq = 0.5; %Hz
nat_freq_rad = 2*pi*nat_freq;
pos_gain = nat_freq_rad*nat_freq_rad;
vel_gain = nat_freq_rad*2;
K = [pos_gain vel_gain pos_gain vel_gain];
s5c = ss(A5-B5*K,B5,C5,0);
pole(s5c)

bode(s5c)

% root locus
pzmap(s5c)
hold
for i = 0.1:0.1:0.5
 nat_freq = i; %Hz
 nat_freq_rad = 2*pi*nat_freq;
 pos_gain = nat_freq_rad*nat_freq_rad;
 vel_gain = nat_freq_rad*2;
 K = [pos_gain vel_gain pos_gain vel_gain];
 s5c = ss(A5-B5*K,B5,C5,0);
 pole(s5c)
 pzmap(s5c)
end
hold