clear all;
close all;

% determination of nominal plant P0(s) by physical model

J_M = 7.80E-05;
f_r = 26.3; % resonance frequency (Hz)
f_a = 8.32; % 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 = 1.5E-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 data/servo1.dat; % frequency response data by operating the drive-side servomotor
%load data/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));
P_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);
P_g = frd(P_jw,w);

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

figure(1);
bode(P_g, 'b.-', P0_g, 'r');
legend('exp.', 'nominal plant');

print -depsc data/nominal.eps; % save as EPS format
print -djpeg -r0 data/nominal.jpg; % save as JPEG format
print -dpng -r0 data/nominal.png; % save as PNG format
save data/nominal.mat P0 P_g P0_g w
