color = lines(1); % Plotting color.
color = color(1,:);

hFig = figure( 1);
hold all;xlabel('X');ylabel('Y');box on;
plot(Y_trace(1,:),Y_trace(2,:),'LineStyle','none','Marker','.','Color',color);
plot(XU(1,:),XU(2,:),'Color',color);
axis tight;
T2=[];
for i = 1:numel(T)-1;
	T2(end+1) = T(i);
	T2(end+1) = (T(i+1)+T(i))/2;
end
    

hFig = figure(2);
set(hFig, 'name','Trace X.')
set(hFig, 'position', [150 200 1200 400]);
hold all;
box on;
scaling = 1;
plot(T,XU(1,:) * scaling,'LineWidth',2,'Color',color);   
plot(T,( Y_trace(1,:) + em_max(1) ) * scaling,'Color','red','LineWidth',2);
plot(T,( Y_trace(1,:) - em_max(1) ) * scaling,'Color','red','LineWidth',2);
ylabel('X [m]');
axis tight; grid on;
xlabel('Time [s]');


hFig = figure(3);
set(hFig, 'name','Trace Y.');
set(hFig, 'position', [150 200 1200 400]);
hold all;
box on;
scaling = 1;
plot(T,XU(2,:),'LineWidth',2,'Color',color);   
plot(T,( Y_trace(2,:) + em_max(2) ) * scaling,'Color','red','LineWidth',2);
plot(T,( Y_trace(2,:) - em_max(2) ) * scaling,'Color','red','LineWidth',2);
ylabel('Y [m]');
axis tight; grid on;
xlabel('Time [s]');


hFig = figure(4);
set(hFig, 'name','Trace \psi.');
set(hFig, 'position', [150 200 1200 400]);
hold all;
box on;
scaling = 180/pi;
plot(T,XU(3,:) * scaling,'LineWidth',2,'Color',color);   
plot(T,( Y_trace(3,:) + em_max(3) ) * scaling,'Color','red','LineWidth',2);
plot(T,( Y_trace(3,:) - em_max(3) ) * scaling,'Color','red','LineWidth',2);
ylabel('\psi [°]');
axis tight; grid on;
xlabel('Time [s]');


hFig = figure(5);
set(hFig, 'name','Trace V_x.');
set(hFig, 'position', [150 200 1200 400]);
hold all;
box on;
scaling = 1;
plot(T,XU(4,:) * scaling,'LineWidth',2,'Color',color);   
plot(T,( Y_trace(4,:) + em_max(4) ) * scaling,'Color','red','LineWidth',2);
plot(T,( Y_trace(4,:) - em_max(4) ) * scaling,'Color','red','LineWidth',2);
ylabel('V_x [m/s]');
axis tight; grid on;
xlabel('Time [s]');


hFig = figure(6);
set(hFig, 'name','Trace V_y.');
set(hFig, 'position', [150 200 1200 400]);
hold all;
box on;
scaling = 1;
plot(T,XU(5,:) * scaling,'LineWidth',2,'Color',color);   
plot(T,( Y_trace(5,:) + em_max(5) ) * scaling,'Color','red','LineWidth',2);
plot(T,( Y_trace(5,:) - em_max(5) ) * scaling,'Color','red','LineWidth',2);
ylabel('V_y [m/s]');
axis tight; grid on;
xlabel('Time [s]');


hFig = figure(7);
set(hFig, 'name','Trace \omega.');
set(hFig, 'position', [150 200 1200 400]);
hold all;
box on;
scaling = 180/pi;
plot(T,XU(6,:) * scaling,'LineWidth',2,'Color',color);   
plot(T,( Y_trace(6,:) + em_max(6) ) * scaling,'Color','red','LineWidth',2);
plot(T,( Y_trace(6,:) - em_max(6) ) * scaling,'Color','red','LineWidth',2);
ylabel('\omega [rad/s]');
axis tight; grid on;
xlabel('Time [s]');


hFig = figure(8);
set(hFig, 'name','Trace a_x.');
set(hFig, 'position', [150 200 1200 400]);
hold all;
box on;
scaling = 1;
plot(T,XU(7,:) * scaling,'LineWidth',2,'Color',color);   
plot(T,( Y_trace(7,:) + em_max(7) ) * scaling,'Color','red','LineWidth',2);
plot(T,( Y_trace(7,:) - em_max(7) ) * scaling,'Color','red','LineWidth',2);
ylabel('a_x [m/s^2]');
axis tight; grid on;
xlabel('Time [s]');


hFig = figure(9);
set(hFig, 'name','Trace \delta.');
set(hFig, 'position', [150 200 1200 400]);
hold all;
box on;
scaling = 180/pi;
plot(T,XU(8,:) * scaling,'LineWidth',2,'Color',color);   
plot(T,( Y_trace(8,:) + em_max(8) ) * scaling,'Color','red','LineWidth',2);
plot(T,( Y_trace(8,:) - em_max(8) ) * scaling,'Color','red','LineWidth',2);
ylabel('\delta [°]');
axis tight; grid on;
xlabel('Time [s]');


hFig = figure(10);
set(hFig, 'name','Trace F_{x_f}.');
set(hFig, 'position', [150 200 1200 400]);
hold all;
box on;
plot(T2,E(1:3:end),'Color',color);
plot(T2([1,end]),ed_max([1,1]),'Color','red','LineWidth',2);
plot(T2([1,end]),-ed_max([1,1]),'Color','red','LineWidth',2);
ylabel('f_{xf}[m/s^2]');
axis tight; grid on;
xlabel('Time [s]');
    
    
hFig = figure(11);
set(hFig, 'name','Trace F_{x_f}.');
set(hFig, 'position', [150 200 1200 400]);
hold all;
box on;
plot(T2,E(2:3:end),'Color',color);
plot(T2([1,end]),ed_max([2,2]),'Color','red','LineWidth',2);
plot(T2([1,end]),-ed_max([2,2]),'Color','red','LineWidth',2);
ylabel('f_{yf}[m/s^2]');
axis tight; grid on;
xlabel('Time [s]');


hFig = figure(12);
set(hFig, 'name','Trace F_{x_f}.');
set(hFig, 'position', [150 200 1200 400]);
hold all;
box on;
plot(T2,E(3:3:end),'Color',color);
plot(T2([1,end]),ed_max([3,3]),'Color','red','LineWidth',2);
plot(T2([1,end]),-ed_max([3,3]),'Color','red','LineWidth',2);
ylabel('f_{yr}[m/s^2]');
axis tight; grid on;
xlabel('Time [s]');
    
% 
% 
% labels = {'e_X[m]','e_Y[m]','e_\psi[°]','e_{vx}[m/s]','e_{vy}[m/s]','e_{\omega}[°/s]','e_{ax}[m/s^2]','e_{\delta}[°]'};
% scale = [1,1,180/pi,1,1,180/pi,1,180/pi];
% 
% 
% i =  1;
% hFig = figure(13);
% set(hFig, 'name','Trace error e_X[m]');
% set(hFig, 'position', [150 200 1200 400]);
% hold all; box on;
% plot(T,(XU(i,:)-Y_trace(i,:)).*scale(i),'Color',color);
% plot(T([1,end]),em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
% plot(T([1,end]),-em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
% axis tight; grid on; ylabel('e_X[m]'); xlabel('Time [s]');
% 
% 
% i =  2;
% hFig = figure(14);
% set(hFig, 'name','Trace error e_Y[m]');
% set(hFig, 'position', [150 200 1200 400]);
% hold all; box on;
% plot(T,(XU(i,:)-Y_trace(i,:)).*scale(i),'Color',color);
% plot(T([1,end]),em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
% plot(T([1,end]),-em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
% axis tight; grid on; ylabel(labels{i}); xlabel('Time [s]');
% 
% 
% i =  3;
% hFig = figure(15);
% set(hFig, 'name','Trace error e_\psi[°]');
% set(hFig, 'position', [150 200 1200 400]);
% hold all; box on;
% plot(T,(XU(i,:)-Y_trace(i,:)).*scale(i),'Color',color);
% plot(T([1,end]),em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
% plot(T([1,end]),-em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
% axis tight; grid on; ylabel(labels{i}); xlabel('Time [s]');
% 
% 
% i =  4;
% hFig = figure(16);
% set(hFig, 'name','Trace error e_{vx}[m/s]');
% set(hFig, 'position', [150 200 1200 400]);
% hold all; box on;
% plot(T,(XU(i,:)-Y_trace(i,:)).*scale(i),'Color',color);
% plot(T([1,end]),em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
% plot(T([1,end]),-em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
% axis tight; grid on; ylabel(labels{i}); xlabel('Time [s]');
% 
% 
% i =  5;
% hFig = figure(17);
% set(hFig, 'name','Trace error e_{vy}[m/s]');
% set(hFig, 'position', [150 200 1200 400]);
% hold all; box on;
% plot(T,(XU(i,:)-Y_trace(i,:)).*scale(i),'Color',color);
% plot(T([1,end]),em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
% plot(T([1,end]),-em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
% axis tight; grid on; ylabel(labels{i}); xlabel('Time [s]');
% 
% 
% i =  6;
% hFig = figure(18);
% set(hFig, 'name','Trace error e_{\omega}[°/s]');
% set(hFig, 'position', [150 200 1200 400]);
% hold all; box on;
% plot(T,(XU(i,:)-Y_trace(i,:)).*scale(i),'Color',color);
% plot(T([1,end]),em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
% plot(T([1,end]),-em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
% axis tight; grid on; ylabel(labels{i}); xlabel('Time [s]');
% 
% 
% i =  7;
% hFig = figure(19);
% set(hFig, 'name','Trace error e_{ax}[m/s^2]');
% set(hFig, 'position', [150 200 1200 400]);
% hold all; box on;
% plot(T,(XU(i,:)-Y_trace(i,:)).*scale(i),'Color',color);
% plot(T([1,end]),em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
% plot(T([1,end]),-em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
% axis tight; grid on; ylabel(labels{i}); xlabel('Time [s]');
% 
% 
% i =  8;
% hFig = figure(20);
% set(hFig, 'name','Trace error e_{\delta}[°]');
% set(hFig, 'position', [150 200 1200 400]);
% hold all; box on;
% plot(T,(XU(i,:)-Y_trace(i,:)).*scale(i),'Color',color);
% plot(T([1,end]),em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
% plot(T([1,end]),-em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
% axis tight; grid on; ylabel(labels{i}); xlabel('Time [s]');