% physical model
J_M = 2.63E-05;
f_r = 50; % resonance frequency (Hz)
f_a = 14; % anti-resonance frequency (Hz)
K_S = (2*pi)^2*J_M*(f_r^2 - f_a^2);
J_L = K_S / (2*pi*f_a)^2;
C_S = 3E-04;
C_L = 5.0E-04;

P_a = [0, 1, -1;
	-K_S/J_M, -C_S/J_M, C_S/J_M;
	K_S/J_L, C_S/J_L, -(C_S+C_L)/J_L];
P_b = [0, 0; 0, 1/J_M; 1/J_L, 0];
P_c = [0, 0,  1; 0, 1, 0];
P_d = [0, 0; 0, 0];

P = ss(P_a, P_b, P_c, P_d);

load servo1.dat; % frequency response data by operating the drive-side servomotor
load servo2.dat; % frequency response data by operating the driven-side servomotor

f = servo1(:,1); % frequency data
w = 2*pi*f;
n = size(servo1,1); % number of frequencies

Gzw_jw = servo2(:,4).*exp(i*pi/180.0*servo2(:,5));
Gyw_jw = servo2(:,2).*exp(i*pi/180.0*servo2(:,3));
Gzu_jw = servo1(:,4).*exp(i*pi/180.0*servo1(:,5));
Gyu_jw = servo1(:,2).*exp(i*pi/180.0*servo1(:,3));

for k = 1:n
  resp(1,1,k) = Gzw_jw(k);
  resp(1,2,k) = Gzu_jw(k);
  resp(2,1,k) = Gyw_jw(k);
  resp(2,2,k) = Gyu_jw(k);
end
G_g = frd(resp, w);

G0 = P; % physical model is used as the nominal plant
G0 = ss(G0.a, G0.b, G0.c, G0.d);
G0_g = frd(G0, w);

bode(G_g, 'b.-', G0_g, 'r');
legend('exp.', 'nominal plant');
