
% ax laterale ay verticale, az orizzontale
clear;
clc;
fullScriptPath = matlab.desktop.editor.getActiveFilename();
[folderPath, ~, ~] = fileparts(fullScriptPath);
% FILE1
%nomefile
file1 = 'C2_2025-05-20_16-15-24.txt';
% Define the names of the files you want to read
filename1 = fullfile(folderPath, file1);
% Importa i dati
data_import_1 = importdata(filename1,'\t');
% Visualizza i dati
data1=data_import_1.data(5:end,:);
disp(data_import_1);
% FILE2
%nomefile
file2 = 'DRONE_2025-05-20_16-15-24.txt';
% Define the names of the files you want to read
filename2 = fullfile(folderPath, file2);
% Importa i dati
data_import_2 = importdata(filename2,'\t');
% Visualizza i dati
data2=data_import_2.data(5:end,:);
disp(data_import_2);

% proiezione mercatore
x1=6371*(data1(:,3)-data1(1,3));
y1=6371*log(tan(pi/4+(data1(:,2)-data1(1,2))/2));


x2=6371*(data2(:,3)-data2(1,3));
y2=6371*log(tan(pi/4+(data2(:,2)-data2(1,2))/2));

%%
figure()
plot3(x1,y1,data1(:,4)*0.3048,'k', 'LineWidth', 1.5);
hold on
plot3(x2,y2,data2(:,4)*0.3048,'--k', 'LineWidth', 1.5)
hold off
grid on
xlabel('X');
ylabel('Y');
zlabel('Z');
pbaspect([1 1 0.75]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 600, 600]) % [x, y, width, height]

figure()
plot(x1,data1(:,4)*0.3048,'k', 'LineWidth', 1.5)
hold on
plot(x2,data2(:,4)*0.3048,'--k', 'LineWidth', 1.5)
hold off
grid on
xlabel('X');
ylabel('Z');
pbaspect([1 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 600, 600]) % [x, y, width, height]

figure()
plot(x1,y1,'k', 'LineWidth', 1.5)
hold on
plot(x2,y2,'--k', 'LineWidth', 1.5)
hold off
grid on
xlabel('X');
ylabel('Y');
pbaspect([1 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 600, 600]) % [x, y, width, height]
%%
figure()
n=4600;
plot3(x1(1:n),y1(1:n),data1(1:n,4)*0.3048,'k', 'LineWidth', 1.5);
hold on
plot3(x2(1:n),y2(1:n),data2(1:n,4)*0.3048,'--k', 'LineWidth', 1.5)
hold off
grid on
xlabel('X');
ylabel('Y');
zlabel('Z');
pbaspect([1 1 0.75]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 600, 600]) % [x, y, width, height]

%NED
figure()
subplot(3,1,1)
plot(data1(1:n,1),data1(1:n,21),'k', 'LineWidth', 1.5)
ylabel('NORD [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data1(1:n,1),data1(1:n,19),'k', 'LineWidth', 1.5)
ylabel('EAST [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data1(1:n,1),data1(1:n,17),'k', 'LineWidth', 1.5)
ylabel('DOWN [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]

%acc
figure()
subplot(3,1,1)
plot(data2(1:n,1),data2(1:n,24)*0.3048/9.8,'k', 'LineWidth', 1.5)
ylabel('a_x [g]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data2(1:n,1),data2(1:n,25)*0.3048/9.8,'k', 'LineWidth', 1.5)
ylabel('a_y [g]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data2(1:n,1),data2(1:n,26)*0.3048/9.8,'k', 'LineWidth', 1.5)
ylabel('a_z [g]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]

%control
figure()
subplot(3,1,1)
plot(data2(1:n,1),data2(1:n,12),'k', 'LineWidth', 1.5)
ylabel('\delta_e [grad]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data2(1:n,1),data2(1:n,14),'k', 'LineWidth', 1.5)
ylabel('\delta_a [grad]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data2(1:n,1),data2(1:n,23),'k', 'LineWidth', 1.5)
ylabel('throttle [%]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]

%%
figure()
n=5000;
m=5700;
plot3(x1(n:m),y1(n:m),data1(n:m,4)*0.3048,'k', 'LineWidth', 1.5);
hold on
plot3(x2(n:m),y2(n:m),data2(n:m,4)*0.3048,'--k', 'LineWidth', 1.5)
hold off
grid on
xlabel('X');
ylabel('Y');
zlabel('Z');
pbaspect([1 1 0.75]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 600, 600]) % [x, y, width, height]
zlim([800 1300])
%NED
figure()
subplot(3,1,1)
plot(data1(n:m,1),data1(n:m,21),'k', 'LineWidth', 1.5)
ylabel('NORD [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data1(n:m,1),data1(n:m,19),'k', 'LineWidth', 1.5)
ylabel('EAST [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data1(n:m,1),data1(n:m,17),'k', 'LineWidth', 1.5)
ylabel('DOWN [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]

%acc
figure()
subplot(3,1,1)
plot(data2(n:m,1),data2(n:m,24)*0.3048/9.8,'k', 'LineWidth', 1.5)
ylabel('a_x [g]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data2(n:m,1),data2(n:m,25)*0.3048/9.8,'k', 'LineWidth', 1.5)
ylabel('a_y [g]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data2(n:m,1),data2(n:m,26)*0.3048/9.8,'k', 'LineWidth', 1.5)
ylabel('a_z [g]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]

%control
figure()
subplot(3,1,1)
plot(data2(n:m,1),data2(n:m,12),'k', 'LineWidth', 1.5)
ylabel('\delta_e [grad]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data2(n:m,1),data2(n:m,14),'k', 'LineWidth', 1.5)
ylabel('\delta_a [grad]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data2(n:m,1),data2(n:m,23),'k', 'LineWidth', 1.5)
ylabel('throttle [%]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]

%%
figure()
n=5700;
m=6500;
plot3(x1(n:m),y1(n:m),data1(n:m,4)*0.3048,'k', 'LineWidth', 1.5);
hold on
plot3(x2(n:m),y2(n:m),data2(n:m,4)*0.3048,'--k', 'LineWidth', 1.5)
hold off
grid on
xlabel('X');
ylabel('Y');
zlabel('Z');
pbaspect([1 1 0.75]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 600, 600]) % [x, y, width, height]
zlim([800 1400])

%NED
figure()
subplot(3,1,1)
plot(data1(n:m,1),data1(n:m,21),'k', 'LineWidth', 1.5)
ylabel('NORD [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
plot(data1(n:m,1),data1(n:m,19),'k', 'LineWidth', 1.5)
ylabel('EAST [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data1(n:m,1),data1(n:m,17),'k', 'LineWidth', 1.5)
ylabel('DOWN [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]

%acc
figure()
subplot(3,1,1)
plot(data2(n:m,1),data2(n:m,24)*0.3048/9.8,'k', 'LineWidth', 1.5)
ylabel('a_x [g]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data2(n:m,1),data2(n:m,25)*0.3048/9.8,'k', 'LineWidth', 1.5)
ylabel('a_y [g]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data2(n:m,1),data2(n:m,26)*0.3048/9.8,'k', 'LineWidth', 1.5)
ylabel('a_z [g]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]

%control
figure()
subplot(3,1,1)
plot(data2(n:m,1),data2(n:m,12),'k', 'LineWidth', 1.5)
ylabel('\delta_e [grad]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data2(n:m,1),data2(n:m,14),'k', 'LineWidth', 1.5)
ylabel('\delta_a [grad]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data2(n:m,1),data2(n:m,23),'k', 'LineWidth', 1.5)
ylabel('throttle [%]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]

%%
figure()
n=7200;
m=7600;
plot3(x1(n:m),y1(n:m),data1(n:m,4)*0.3048,'k', 'LineWidth', 1.5);
hold on
plot3(x2(n:m),y2(n:m),data2(n:m,4)*0.3048,'--k', 'LineWidth', 1.5)
hold off
grid on
xlabel('X');
ylabel('Y');
zlabel('Z');
pbaspect([1 1 0.75]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 600, 600]) % [x, y, width, height]
zlim([800 2000])

%NED
figure()
subplot(3,1,1)
plot(data1(n:m,1),data1(n:m,21),'k', 'LineWidth', 1.5)
ylabel('NORD [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data1(n:m,1),data1(n:m,19),'k', 'LineWidth', 1.5)
ylabel('EAST [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data1(n:m,1),data1(n:m,17),'k', 'LineWidth', 1.5)
ylabel('DOWN [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]

%acc
figure()
subplot(3,1,1)
plot(data2(n:m,1),data2(n:m,24)*0.3048/9.8,'k', 'LineWidth', 1.5)
ylabel('a_x [g]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data2(n:m,1),data2(n:m,25)*0.3048/9.8,'k', 'LineWidth', 1.5)
ylabel('a_y [g]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data2(n:m,1),data2(n:m,26)*0.3048/9.8,'k', 'LineWidth', 1.5)
ylabel('a_z [g]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]

%control
figure()
subplot(3,1,1)
plot(data2(n:m,1),data2(n:m,12),'k', 'LineWidth', 1.5)
ylabel('\delta_e [grad]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data2(n:m,1),data2(n:m,14),'k', 'LineWidth', 1.5)
ylabel('\delta_a [grad]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data2(n:m,1),data2(n:m,23),'k', 'LineWidth', 1.5)
ylabel('throttle [%]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]

%%
figure()
n=7600;
m=10100;
plot3(x1(n:m),y1(n:m),data1(n:m,4)*0.3048,'k', 'LineWidth', 1.5);
hold on
plot3(x2(n:m),y2(n:m),data2(n:m,4)*0.3048,'--k', 'LineWidth', 1.5)
hold off
grid on
xlabel('X');
ylabel('Y');
zlabel('Z');
pbaspect([1 1 0.75]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 600, 600]) % [x, y, width, height]
zlim([800 4000])

%NED
figure()
subplot(3,1,1)
plot(data1(n:m,1),data1(n:m,21),'k', 'LineWidth', 1.5)
ylabel('NORD [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data1(n:m,1),data1(n:m,19),'k', 'LineWidth', 1.5)
ylabel('EAST [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data1(n:m,1),data1(n:m,17),'k', 'LineWidth', 1.5)
ylabel('DOWN [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]

%acc
figure()
subplot(3,1,1)
plot(data2(n:m,1),data2(n:m,24)*0.3048/9.8,'k', 'LineWidth', 1.5)
ylabel('a_x [g]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data2(n:m,1),data2(n:m,25)*0.3048/9.8,'k', 'LineWidth', 1.5)
ylabel('a_y [g]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data2(n:m,1),data2(n:m,26)*0.3048/9.8,'k', 'LineWidth', 1.5)
ylabel('a_z [g]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]

%control
figure()
subplot(3,1,1)
plot(data2(n:m,1),data2(n:m,12),'k', 'LineWidth', 1.5)
ylabel('\delta_e [grad]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data2(n:m,1),data2(n:m,14),'k', 'LineWidth', 1.5)
ylabel('\delta_a [grad]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data2(n:m,1),data2(n:m,23),'k', 'LineWidth', 1.5)
ylabel('throttle [%]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]