-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathplot_results.m
96 lines (88 loc) · 2.99 KB
/
plot_results.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
clear all
unzip('results.zip','./results');
file_name = '2023_10_26_06_24';
source = readmatrix(['./results/',file_name,'.csv']);
time = source(:,1);
ref_x = source(:,2);
x = source(:,3);
ref_y = source(:,4);
y = source(:,5);
ref_z = source(:,6);
z = source(:,7);
ref_u = source(:,8);
u = source (:,9);
ref_v = source(:,10);
v = source(:,11);
ref_w = source(:,12);
w = source(:,13);
ref_phi = rad2deg(source(:,14));
phi = rad2deg(source(:,15));
ref_theta = rad2deg(source(:,16));
theta = rad2deg(source(:,17));
ref_psi = rad2deg(source(:,18));
psi = rad2deg(source(:,19));
thrust = source(:,20);
fig1 = figure('Name','Position Tracking Results','Position',[700 250 900 600]);
tiledlayout(3,1,'Padding','none');
nexttile
plot(time,x,'Color',[0 0.4470 0.7410],'LineWidth',2);
hold on
plot(time,ref_x,'Color','k','LineStyle','--','LineWidth',2);
legend(file_name,'Reference','Location','northoutside','Orientation','horizontal');
ylabel('X Position [m]');
set(gca,'FontSize',17,'LineWidth',1.5);
nexttile
plot(time,y,'Color',[0 0.4470 0.7410],'LineWidth',2);
hold on
plot(time,ref_y,'Color','k','LineStyle','--','LineWidth',2);
ylabel('Y Position [m]');
set(gca,'FontSize',17,'LineWidth',1.5);
nexttile
plot(time,z,'Color',[0 0.4470 0.7410],'LineWidth',2);
hold on
plot(time,ref_z,'Color','k','LineStyle','--','LineWidth',2);
ylabel('Z Position [m]');
set(gca,'FontSize',17,'LineWidth',1.5);
fig2 = figure('Name','Velocity Tracking Results','Position',[700 250 900 600]);
tiledlayout(3,1,'Padding','none');
nexttile
plot(time,u,'Color',[0 0.4470 0.7410],'LineWidth',2);
hold on
plot(time,ref_u,'Color','k','LineStyle','--','LineWidth',2);
legend(file_name,'Reference','Location','northoutside','Orientation','horizontal');
ylabel('X Velocity [m/s]');
set(gca,'FontSize',17,'LineWidth',1.5);
nexttile
plot(time,v,'Color',[0 0.4470 0.7410],'LineWidth',2);
hold on
plot(time,ref_v,'Color','k','LineStyle','--','LineWidth',2);
ylabel('Y Velocity [m/s]');
set(gca,'FontSize',17,'LineWidth',1.5);
nexttile
plot(time,w,'Color',[0 0.4470 0.7410],'LineWidth',2);
hold on
plot(time,ref_w,'Color','k','LineStyle','--','LineWidth',2);
ylabel('Z Velocity [m/s]');
set(gca,'FontSize',17,'LineWidth',1.5);
fig3 = figure('Name','Attitude Tracking Results','Position',[700 250 900 600]);
tiledlayout(3,1,'Padding','none');
nexttile
plot(time,phi,'Color',[0 0.4470 0.7410],'LineWidth',2);
hold on
plot(time,ref_phi,'Color','k','LineStyle','--','LineWidth',2);
legend(file_name,'Reference','Location','northoutside','Orientation','horizontal');
ylabel('Phi Angle [deg]');
set(gca,'FontSize',17,'LineWidth',1.5);
nexttile
plot(time,theta,'Color',[0 0.4470 0.7410],'LineWidth',2);
hold on
plot(time,ref_theta,'Color','k','LineStyle','--','LineWidth',2);
ylabel('Theta Angle [deg]');
set(gca,'FontSize',17,'LineWidth',1.5);
nexttile
plot(time,psi,'Color',[0 0.4470 0.7410],'LineWidth',2);
hold on
plot(time,ref_psi,'Color','k','LineStyle','--','LineWidth',2);
ylabel('Psi Angle [deg]');
set(gca,'FontSize',17,'LineWidth',1.5);
% saveas(fig1,'position_tracking_results.png');