MCSProject/plotter.m at main · bluesealin/MCSProject · GitHub

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
%% plotter

path = 'figs' ;
if ~exist(ful, 'dir')
    mkdir(pwd, 'figs');
end
% Simulation Scenario 1

% scinario parameters
sim_num = 0;
labels = {'under fixed SE current' , 'under varying current', 'with reference model', 'without reference model', 'following set of points'} ;
for i= 1:5
    close all
    sim_num = sim_num + 1;

    if i == 1
        simulation = 1 ;
        ref_model = 1 ;
        lbl = labels{i};
    elseif i== 2
        simulation = 2 ;
        ref_model = 1 ;
        lbl = labels{i};
    elseif i ==3
        simulation = 3 ;
        ref_model = 1 ;
        lbl = labels{i};
    elseif i== 4
        simulation = 3 ;
        ref_model = 0 ;
        lbl = labels{i};
    else
        simulation = 4 ;
        ref_model = 1 ;
        lbl = labels{i};
    end


% run the simulation
sim('part1.slx') ;

% data extraction
eta =          logsout.getElement('eta') ;
desired_eta =  logsout.getElement('set_point') ;
time= eta.Values.Time ;

% current pose
north = eta.Values.Data(: , 1) ;
east  = eta.Values.Data(: , 2) ;
psi   = eta.Values.Data(: , 3) ;

% desired pose
des_north = desired_eta.Values.Data(: , 1) ;
des_east  = desired_eta.Values.Data(: , 2) ;
des_psi   = desired_eta.Values.Data(: , 3) ;


% plotting

figure(1)

plot(time, north)
hold on
plot(time, east)
hold on
plot(time, des_north)
hold on
plot(time, des_east)
xlabel('Time (sec)')
ylabel('Amplitude (m)')
title(sprintf('DP %s (Translational motion)', lbl));
legend('North', 'East')
saveas(gcf, fullfile(path , sprintf('Translational_%02d.png', sim_num)));

figure(2)

plot(time, psi)
hold on
plot(time, des_psi )
xlabel('Time (sec)')
ylabel('Amplitude (rad) ')
title(sprintf('DP %s (Rotational motion)', lbl));
saveas(gcf, fullfile(path , sprintf('Rotational_%02d.png', sim_num)));

figure(3)


plot(east, north) ;
hold on
plot(des_east, des_north),
hold on
indices = 1:1000:length(east) ;

selected_x = east(indices);
selected_y = north(indices);
selected_u = cos(-psi + (pi/2)) ;
selected_u = selected_u(indices);
selected_v = sin(-psi + (pi/2)) ;
selected_v = selected_v(indices);

%scatter(selected_x, selected_y, 'filled');
%hold on
quiver(selected_x, selected_y, selected_u, selected_v, 0.5, 'Color', 'r', 'LineWidth', 1.5);


title('XY plot')
xlabel('Amplitude in East direction (m)')
ylabel('Amplitude in North direction (m)')
legend('actual trajectory', 'desired trajectory')
saveas(gcf, fullfile(path , sprintf('xy_%02d.png', sim_num)));


end