-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathstateTraj_plot.m
130 lines (115 loc) · 4.47 KB
/
stateTraj_plot.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
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
function stateTraj_plot(problem,conMode)
x1_max = problem.x1_max;
x1_min = problem.x1_min;
x2_max = problem.x2_max;
x2_min = problem.x2_min;
if conMode == 1
%load data
t_log_struct = load('./matfile_storage/fixed_t_log.mat');
x_log_struct = load('./matfile_storage/fixed_x_log.mat');
u_log_struct = load('./matfile_storage/fixed_u_log.mat');
x_tk_log = load('./matfile_storage/fixed_x_tk_log.mat');
t_update_log = load('./matfile_storage/fixed_t_update_log.mat');
x1_min_log_struct = load('./matfile_storage/fixed_x1_min_log.mat');
x1_max_log_struct = load('./matfile_storage/fixed_x1_max_log.mat');
x2_min_log_struct = load('./matfile_storage/fixed_x2_min_log.mat');
x2_max_log_struct = load('./matfile_storage/fixed_x2_max_log.mat');
%Extract Vector
x_log = x_log_struct(1).x_log;
t_log = t_log_struct(1).t_log;
u_log = u_log_struct(1).u_log;
x1_min_log = x1_min_log_struct(1).x1_min_log;
x1_max_log = x1_max_log_struct(1).x1_max_log;
x2_min_log = x2_min_log_struct(1).x2_min_log;
x2_max_log = x2_max_log_struct(1).x2_max_log;
x1_update = x_tk_log(1).x_update(:,1);
x2_update = x_tk_log(1).x_update(:,2);
t_update = t_update_log(1).t_update;
x_update = [x1_update,x2_update];
% Plot x1, x2 vs time for Periodic Control
figure('DefaultAxesFontSize',24)
subplot(1,3,1)
hold on
plot(t_log,x_log(:,1),'LineWidth',2)
plot(t_log,x1_min_log,'r--','LineWidth',2)
plot(t_log,x1_max_log,'m:','LineWidth',2)
plot(t_update,x_update(:,1),'bo','MarkerSize',10)
legend('x_1 trajectory','x_{1,min}','x_{1,max}','Control Update')
xlabel('t')
ylabel('x1')
title('Position x_1 over time')
ylim([x1_min-2,x1_max+2])
subplot(1,3,2)
hold on
plot(t_log,x_log(:,2),'LineWidth',2)
plot(t_log,x2_min_log,'r--','LineWidth',2)
plot(t_log,x2_max_log,'m:','LineWidth',2)
plot(t_update,x_update(:,2),'bo','MarkerSize',10)
legend('x_2 trajectory','x_{2,min}','x_{2,max}','Control Update')
xlabel('t')
ylabel('x2')
title('Velocity x_2 over time')
ylim([x2_min-2,x2_max+2])
subplot(1,3,3)
hold on
plot(t_log,u_log,'LineWidth',2)
legend('Fixed Time Control')
xlabel('t')
ylabel('u')
title('Control u over time')
elseif conMode == 2
%load data
t_log_struct = load('./matfile_storage/selfTriggered_t_log.mat');
x_log_struct = load('./matfile_storage/selfTriggered_x_log.mat');
u_log_struct = load('./matfile_storage/selfTriggered_u_log.mat');
x_tk_log = load('./matfile_storage/selfTriggered_x_tk_log.mat');
t_update_struct = load('./matfile_storage/selfTriggered_t_update.mat');
x1_min_log_struct = load('./matfile_storage/selfTriggered_x1_min_log.mat');
x1_max_log_struct = load('./matfile_storage/selfTriggered_x1_max_log.mat');
x2_min_log_struct = load('./matfile_storage/selfTriggered_x2_min_log.mat');
x2_max_log_struct = load('./matfile_storage/selfTriggered_x2_max_log.mat');
%Extract vector from data
x_log = x_log_struct(1).x_log;
t_log = t_log_struct(1).t_log;
u_log = u_log_struct(1).u_log;
x1_min_log = x1_min_log_struct(1).x1_min_log;
x1_max_log = x1_max_log_struct(1).x1_max_log;
x2_min_log = x2_min_log_struct(1).x2_min_log;
x2_max_log = x2_max_log_struct(1).x2_max_log;
x1_update = x_tk_log(1).x_update(:,1);
x2_update = x_tk_log(1).x_update(:,2);
t_update = t_update_struct(1).t_update;
x_update = [x1_update,x2_update];
% Plot x1, x2 vs time for Self-triggered Control
figure('DefaultAxesFontSize',24)
subplot(1,3,1)
hold on
plot(t_log,x_log(:,1),'LineWidth',2)
plot(t_log,x1_min_log,'r--','LineWidth',2)
plot(t_log,x1_max_log,'m:','LineWidth',2)
plot(t_update,x_update(:,1),'bo','MarkerSize',10)
legend('x_1 trajectory','x_{1,min}','x_{1,max}','Control Update')
xlabel('t')
ylabel('x1')
title('Position x_1 over time')
ylim([x1_min-2,x1_max+2])
subplot(1,3,2)
hold on
plot(t_log,x_log(:,2),'LineWidth',2)
plot(t_log,x2_min_log,'r--','LineWidth',2)
plot(t_log,x2_max_log,'m:','LineWidth',2)
plot(t_update,x_update(:,2),'bo','MarkerSize',10)
legend('x_2 trajectory','x_{2,min}','x_{2,max}','Control Update')
xlabel('t')
ylabel('x2')
title('Velocity x_2 over time')
ylim([x2_min-2,x2_max+2])
subplot(1,3,3)
hold on
plot(t_log,u_log,'LineWidth',2)
legend('Self-Triggered Control')
xlabel('t')
ylabel('u')
title('Control u over time')
end
end