
% These are to plot the results of the first test case including
% disturbances, which presented in the figures (23-25).
%
% x_fpid: FPID output x position
% y_fpid: FPID output y position
% z_fpid: FPID output z position
% phi_fpid: FPID output roll angle
% theta_fpid: FPID output pitch angle
% psi_fpid: FPID output yaw angle
%
% x_pid: PID output x position
% y_pid: PID output y position
% z_pid: PID output z position
% phi_pid: PID output roll angle
% theta_pid: PID output pitch angle
% psi_pid: PID output yaw angle
%
% x_ref: Reference waypoints for x position
% y_ref: Reference waypoints for y position
% z_ref: Reference waypoints for z position
%
% white_noise: additive white Gaussian noise added to the disturbance wave
% dist_wave: disturbance wave
%
% dist_b_u: disturbance effect decomposed in body frame x-axis
% dist_b_v: disturbance effect decomposed in body frame y-axis
% dist_b_w: disturbance effect decomposed in body frame z-axis
%
% uc: disturbance effect decomposed in inertial NED frame x-axis
% vc: disturbance effect decomposed in inertial NED frame y-axis
% wc: disturbance effect decomposed in inertial NED frame z-axis

figure(1);
subplot(2,1,1);
plot(x_fpid.signals.values, y_fpid.signals.values,'r', x_pid.signals.values, y_pid.signals.values, x_ref.signals.values, y_ref.signals.values);
xlabel('x coordinates (m) ');
ylabel('y coordinates (m) ');
legend('Fuzzy PID','PID','Reference');

subplot(2,1,2);
plot(psi_fpid.time, psi_fpid.signals.values, 'r', psi_pid.time, psi_pid.signals.values, psi_ref.time, psi_ref.signals.values);
ylabel(' yaw orientation (rad) ');
xlabel('time (sec) ');
legend('Fuzzy PID','PID','Reference');


figure(2);
plot(white_noise.time, white_noise.signals.values, dist_wave.time, dist_wave.signals.values);
ylabel(' current noise ');
xlabel(' time (s) ');


figure(3);
subplot(3,1,1);
plot(dist_b_u.time, dist_b_u.signals.values);
ylabel(' surge disturbance (b-frame) ');

subplot(3,1,2);
plot(dist_b_v.time, dist_b_v.signals.values);
ylabel(' sway disturbance (b-frame) ');

subplot(3,1,3);
plot(dist_b_w.time, dist_b_w.signals.values);
ylabel(' heave disturbance (b-frame) ');
xlabel('time (sec) ');

figure(4);
subplot(3,1,1);
plot(uc.time, uc.signals.values, 'r');
ylabel(' surge disturbance (n-frame) ');

subplot(3,1,2);
plot(vc.time, vc.signals.values, 'r');
ylabel(' sway disturbance (n-frame) ');

subplot(3,1,3);
plot(wc.time, wc.signals.values, 'r');
ylabel(' heave disturbance (n-frame) ');
xlabel('time (sec) ');

%time response of x and y
figure(5)
subplot(2,1,1);
plot(x_fpid.time, x_fpid.signals.values, 'r', x_pid.time, x_pid.signals.values,'--', x_ref.time, x_ref.signals.values,':');
ylabel(' X Position (m) ');
legend('Fuzzy PID','PID','Reference');

subplot(2,1,2);
plot(y_fpid.time, y_fpid.signals.values, 'r', y_pid.time, y_pid.signals.values,'--', y_ref.time, y_ref.signals.values,':');
ylabel(' Y Position (m) ');
xlabel(' Time (sec) ');
legend('Fuzzy PID','PID','Reference');