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
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich.
%
% All rights reserved.
%
% Please see the LICENSE file that has been included as part of this package.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [fig_time,axes_time,fig_pos,axes_pos] = plot_trajectory_z(x,u,ctrl_info,params)
fig_time = figure;
% check if input is 3-dimensional
n_traj = size(x,3);
assert(size(u,3) == n_traj);
assert(size(ctrl_info,3) == n_traj);
nx = params.model.nx;
assert(nx == size(x,1));
t = 0:params.model.TimeStep:params.model.TimeStep*params.model.HorizonLength;
% plot
axes_time = cell(4,1);
axes_time{1} = subplot(4,1,1);
hold on;
for i = 1:n_traj
plot(axes_time{1},t,x(1,:,i),'DisplayName',sprintf('z_%d',i));
end
legend('Location','EastOutside')
% max position
x_max = params.constraints.MaxAbsPositionXZ; %*T(1);
plot(axes_time{1}, [t(1); t(end)],[x_max; x_max],'k--','HandleVisibility','off');
plot(axes_time{1}, [t(1); t(end)],[-x_max; -x_max],'k--','HandleVisibility','off');
ylabel('Position [Mm]')
axes_time{2} = subplot(4,1,2);
hold on;
for i = 1:n_traj
plot(axes_time{2},t,x(2,:,i),'DisplayName',sprintf('v_{z%d}',i));
end
legend('Location','EastOutside')
ylabel('Velocity [km/s]')
axes_time{3} = subplot(4,1,3);
hold on;
% append one input value for plotting
u(:,length(t),:) = u(:,length(t)-1,:);
for i = 1:n_traj
stairs(axes_time{3},t,u(1,:,i)','DisplayName',sprintf('u_{z%d}',i));
end
% max thrust
thrust_max = params.constraints.MaxAbsThrust;
plot([t(1); t(end)],[thrust_max; thrust_max],'k--','HandleVisibility','off');
plot([t(1); t(end)],[-thrust_max; -thrust_max],'k--','HandleVisibility','off');
legend('Location','EastOutside')
ylabel('Thrust [N]')
% feasibility
axes_time{4} = subplot(4,1,4);
hold on;
for i = 1:n_traj
% append one input value for plotting
ctrl_feas = [ctrl_info(:,:,i).ctrl_feas];
ctrl_feas(length(t)) = ctrl_feas(length(t) - 1);
stairs(axes_time{4},t,ctrl_feas','DisplayName',sprintf('feas_%d',i));
end
legend('Location','EastOutside')
ylabel('Feasible [0/1]')
% link axes
axes_time = [axes_time{:}];
linkaxes(axes_time,'x')
xlabel('Time [s]')
end
|
