y_tANDx_t.m 14 KB
Newer Older
Farid Alijani's avatar
Farid Alijani committed
1
2
3
4
clc;
close all;
clear all;

Farid Alijani's avatar
Farid Alijani committed
5
6
7
8
9
%% Import Data 

% data 1: 
Pose_Matrix_data_1 = csvread('CAM_PID_.266_.0003_.014.txt',1,0);
Vel_Matrix_data_1 = csvread('vel_PID_.266_.0003_.014.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
10
Raw_meas_1 = csvread('Trans_PID_.266_.0003_.014.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
11
12
13
14
15
16
17
18
19
20

% time in position
TimeP_ros_data_1 = Pose_Matrix_data_1(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  durationP_data_1 = (TimeP_ros_data_1(size(TimeP_ros_data_1,1),:) - TimeP_ros_data_1(1,:))*10^(-9); 
  
t_P_sec_data_1 = 0:durationP_data_1/size(TimeP_ros_data_1,1):durationP_data_1;
t_P_sec_data_1(:,size(t_P_sec_data_1,2)) = [];

Farid Alijani's avatar
Farid Alijani committed
21
22
23
24
25
26
27
28
29
% time in RAW MEASUREMENT
T_Raw_ros_1 = Raw_meas_1(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  duRAW_1 = (T_Raw_ros_1(size(T_Raw_ros_1,1),:) - T_Raw_ros_1(1,:))*10^(-9); 
  
t_Raw_ave_1 = 0:duRAW_1/size(T_Raw_ros_1,1):duRAW_1;
t_Raw_ave_1(:,size(t_Raw_ave_1,2)) = [];

Farid Alijani's avatar
Farid Alijani committed
30
31
32
33
34
35
36
37
38
% time in vel
TimeV_ros_data_1 = Vel_Matrix_data_1(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  durationV_data_1 = (TimeV_ros_data_1(size(TimeV_ros_data_1,1),:) - TimeV_ros_data_1(1,:))*10^(-9);

t_V_sec_data_1 = 0:durationV_data_1/size(TimeV_ros_data_1,1):durationV_data_1;
t_V_sec_data_1(:,size(t_V_sec_data_1,2)) = [];

Farid Alijani's avatar
Farid Alijani committed
39
% -----------------------------------------------------------------------------------------------------
Farid Alijani's avatar
Farid Alijani committed
40
41
42
43
% data 2: 

Pose_Matrix_data_2 = csvread('CAM_PID_.28_0_.006.txt',1,0);
Vel_Matrix_data_2 = csvread('vel_PID_.28_0_.006.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
44
RAW_meas_2 = csvread('Trans_PID_.28_0_.006.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
45
46
47
48
49
50
51
52
53
54

% time in position
TimeP_ros_data_2 = Pose_Matrix_data_2(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  durationP_data_2 = (TimeP_ros_data_2(size(TimeP_ros_data_2,1),:) - TimeP_ros_data_2(1,:))*10^(-9); 
  
t_P_sec_data_2 = 0:durationP_data_2/size(TimeP_ros_data_2,1):durationP_data_2;
t_P_sec_data_2(:,size(t_P_sec_data_2,2)) = [];

Farid Alijani's avatar
Farid Alijani committed
55
56
57
58
59
60
61
62
63
% time in RAW MEASUREMEMT
T_RAW_ros_2 = RAW_meas_2(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  duRAW_2 = (T_RAW_ros_2(size(T_RAW_ros_2,1),:) - T_RAW_ros_2(1,:))*10^(-9); 
  
t_RAW_ave_2 = 0:duRAW_2/size(T_RAW_ros_2,1):duRAW_2;
t_RAW_ave_2(:,size(t_RAW_ave_2,2)) = [];

Farid Alijani's avatar
Farid Alijani committed
64
65
% time in vel
TimeV_ros_data_2 = Vel_Matrix_data_2(:,1); % ros time, needs to be converted to sec...
Farid Alijani's avatar
Farid Alijani committed
66

Farid Alijani's avatar
Farid Alijani committed
67
68
69
70
71
72
73
74
75
76
77
% duriation =         end_time        -     start_time
  durationV_data_2 = (TimeV_ros_data_2(size(TimeV_ros_data_2,1),:) - TimeV_ros_data_2(1,:))*10^(-9);

t_V_sec_data_2 = 0:durationV_data_2/size(TimeV_ros_data_2,1):durationV_data_2;
t_V_sec_data_2(:,size(t_V_sec_data_2,2)) = [];

% -----------------------------------------------------------------------------------------------------
% data 3: 

Pose_Matrix_data_3 = csvread('CAM_PID_.29_.00026_.015.txt',1,0);
Vel_Matrix_data_3 = csvread('vel_PID_.29_.00026_.015.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
78
RAW_meas_3 = csvread('Trans_PID_.29_.00026_.015.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
79
80
81
82
83
84
85
86
87
88

% time in position
TimeP_ros_data_3 = Pose_Matrix_data_3(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  durationP_data_3 = (TimeP_ros_data_3(size(TimeP_ros_data_3,1),:) - TimeP_ros_data_3(1,:))*10^(-9); 
  
t_P_sec_data_3 = 0:durationP_data_3/size(TimeP_ros_data_3,1):durationP_data_3;
t_P_sec_data_3(:,size(t_P_sec_data_3,2)) = [];

Farid Alijani's avatar
Farid Alijani committed
89
90
91
92
93
94
95
96
97
% time in RAW MEASUREMENT
T_RAW_ros_3 = RAW_meas_3(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  duRaw_3 = (T_RAW_ros_3(size(T_RAW_ros_3,1),:) - T_RAW_ros_3(1,:))*10^(-9); 
  
t_Raw_real_3 = 0:duRaw_3/size(T_RAW_ros_3,1):duRaw_3;
t_Raw_real_3(:,size(t_Raw_real_3,2)) = [];

Farid Alijani's avatar
Farid Alijani committed
98
99
100
101
102
103
104
105
106
107
108
109
110
% time in vel
TimeV_ros_data_3 = Vel_Matrix_data_3(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  durationV_data_3 = (TimeV_ros_data_3(size(TimeV_ros_data_3,1),:) - TimeV_ros_data_3(1,:))*10^(-9);

t_V_sec_data_3 = 0:durationV_data_3/size(TimeV_ros_data_3,1):durationV_data_3;
t_V_sec_data_3(:,size(t_V_sec_data_3,2)) = [];

% -----------------------------------------------------------------------------------------------------
% data 4: 
Pose_Matrix_data_4 = csvread('CAM_PID_.3_.0004_.02.txt',1,0);
Vel_Matrix_data_4 = csvread('vel_PID_.3_.0004_.02.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
111
Raw_Meas_4 = csvread('Trans_PID_.3_.0004_.02.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
112
113
114
115
116
117
118
119
120
121

% time in position
TimeP_ros_data_4 = Pose_Matrix_data_4(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  durationP_data_4 = (TimeP_ros_data_4(size(TimeP_ros_data_4,1),:) - TimeP_ros_data_4(1,:))*10^(-9); 
  
t_P_sec_data_4 = 0:durationP_data_4/size(TimeP_ros_data_4,1):durationP_data_4;
t_P_sec_data_4(:,size(t_P_sec_data_4,2)) = [];

Farid Alijani's avatar
Farid Alijani committed
122
123
% time in RAW_MEASUREMENT
T_Raw_ros_4 = Raw_Meas_4(:,1); % ros time, needs to be converted to sec...
Farid Alijani's avatar
Farid Alijani committed
124
125

% duriation =         end_time        -     start_time
Farid Alijani's avatar
Farid Alijani committed
126
  duRaw_4 = (T_Raw_ros_4(size(T_Raw_ros_4,1),:) - T_Raw_ros_4(1,:))*10^(-9); 
Farid Alijani's avatar
Farid Alijani committed
127
  
Farid Alijani's avatar
Farid Alijani committed
128
129
t_Raw_real_4 = 0:duRaw_4/size(T_Raw_ros_4,1):duRaw_4;
t_Raw_real_4(:,size(t_Raw_real_4,2)) = [];
Farid Alijani's avatar
Farid Alijani committed
130

Farid Alijani's avatar
Farid Alijani committed
131
132
133
134
135
136
137
138
139
140
141
% time in vel
TimeV_ros_data_4 = Vel_Matrix_data_4(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  durationV_data_4 = (TimeV_ros_data_4(size(TimeV_ros_data_4,1),:) - TimeV_ros_data_4(1,:))*10^(-9);

t_V_sec_data_4 = 0:durationV_data_4/size(TimeV_ros_data_4,1):durationV_data_4;
t_V_sec_data_4(:,size(t_V_sec_data_4,2)) = [];

%% Pose estimation

Farid Alijani's avatar
Farid Alijani committed
142
%data 1:
Farid Alijani's avatar
Farid Alijani committed
143
144
145
x_raw_1 = Raw_meas_1(:,4); % x_t
y_raw_1 = Raw_meas_1(:,3); % y_t
theta_raw_1 = Raw_meas_1(:,7); % yaw
Farid Alijani's avatar
Farid Alijani committed
146
147
148
149
150
151

Pose_X_data_1 = Pose_Matrix_data_1(:,4);
Pose_Y_data_1 = Pose_Matrix_data_1(:,3);
Theta_data_1 = Pose_Matrix_data_1(:,7);

%data 2:
Farid Alijani's avatar
Farid Alijani committed
152
153
154
x_raw_2 = RAW_meas_2(:,4); % x_t
y_raw_2 = RAW_meas_2(:,3); % y_t
theta_raw_2 = RAW_meas_2(:,7); % yaw
Farid Alijani's avatar
Farid Alijani committed
155
156
157
158

Pose_X_data_2 = Pose_Matrix_data_2(:,4);
Pose_Y_data_2 = Pose_Matrix_data_2(:,3);
Theta_data_2 = Pose_Matrix_data_2(:,7);
Farid Alijani's avatar
Farid Alijani committed
159

Farid Alijani's avatar
Farid Alijani committed
160
%data 3:
Farid Alijani's avatar
Farid Alijani committed
161
162
163
x_raw_3 = RAW_meas_3(:,4); % x_t
y_raw_3 = RAW_meas_3(:,3); % y_t
theta_raw_3 = RAW_meas_3(:,7); % yaw
Farid Alijani's avatar
Farid Alijani committed
164
165
166
167
168
169
170

Pose_X_data_3 = Pose_Matrix_data_3(:,4);
Pose_Y_data_3 = Pose_Matrix_data_3(:,3);
Theta_data_3 = Pose_Matrix_data_3(:,7);


%data 4:
Farid Alijani's avatar
Farid Alijani committed
171
172
173
x_raw_4 = Raw_Meas_4(:,4); % x_t
y_raw_4 = Raw_Meas_4(:,3); % y_t
theta_raw_4 = Raw_Meas_4(:,7); % yaw
Farid Alijani's avatar
Farid Alijani committed
174

Farid Alijani's avatar
Farid Alijani committed
175
176
Pose_X_data_4 = Pose_Matrix_data_4(:,4);
Pose_Y_data_4 = Pose_Matrix_data_4(:,3);
Farid Alijani's avatar
Farid Alijani committed
177
Theta_data_4 = Pose_Matrix_data_4(:,7);
Farid Alijani's avatar
Farid Alijani committed
178
179
180


% Reference Circle 
Farid Alijani's avatar
Farid Alijani committed
181
thresh_X = .0013;
Farid Alijani's avatar
Farid Alijani committed
182
theta = 0:.001:2*pi;
Farid Alijani's avatar
Farid Alijani committed
183

Farid Alijani's avatar
Farid Alijani committed
184
% needs to be adjusted manually if docking platform is replaced!
Farid Alijani's avatar
Farid Alijani committed
185
186
187
ref_x = -.2025;
ref_y = -.0185;

Farid Alijani's avatar
Farid Alijani committed
188
189
190
191
192
x_circle = thresh_X*cos(theta) + ref_x; % ref_X needs to be recorded
y_circle = thresh_X*sin(theta) + ref_y; % ref_Y needs to be recorded

%% Velocity estimation

Farid Alijani's avatar
Farid Alijani committed
193
194
195
196
% data 1:
Vel_X_data_1 = Vel_Matrix_data_1(:,2);
Vel_Y_data_1 = Vel_Matrix_data_1(:,3);
Omega_Z_data_1 = Vel_Matrix_data_1(:,7);
Farid Alijani's avatar
Farid Alijani committed
197
198


Farid Alijani's avatar
Farid Alijani committed
199
200
201
202
% data 2:
Vel_X_data_2 = Vel_Matrix_data_2(:,2);
Vel_Y_data_2 = Vel_Matrix_data_2(:,3);
Omega_Z_data_2 = Vel_Matrix_data_2(:,7);
Farid Alijani's avatar
Farid Alijani committed
203
204


Farid Alijani's avatar
Farid Alijani committed
205
206
207
208
% data 3:
Vel_X_data_3 = Vel_Matrix_data_3(:,2);
Vel_Y_data_3 = Vel_Matrix_data_3(:,3);
Omega_Z_data_3 = Vel_Matrix_data_3(:,7);
Farid Alijani's avatar
Farid Alijani committed
209

Farid Alijani's avatar
Farid Alijani committed
210
211
212
213
214
% data 4:
Vel_X_data_4 = Vel_Matrix_data_4(:,2);
Vel_Y_data_4 = Vel_Matrix_data_4(:,3);
Omega_Z_data_4 = Vel_Matrix_data_4(:,7);
%% Plot
Farid Alijani's avatar
Farid Alijani committed
215
216
217


figure;
Farid Alijani's avatar
Farid Alijani committed
218
set(gcf,'color','white');
Farid Alijani's avatar
Farid Alijani committed
219

Farid Alijani's avatar
Farid Alijani committed
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
% % raw measurement
% subplot(331);
% 
% plot(t_Raw_ave_1,x_raw_1,'b','LineWidth',1.5);
% hold on;
% plot(t_RAW_ave_2,x_raw_2,'k','LineWidth',1.5);
% hold on;
% plot(t_Raw_real_3,x_raw_3,'g','LineWidth',1.5);
% hold on;
% plot(t_Raw_real_4,x_raw_4,'r','LineWidth',1.5);
% ylabel('x_{raw}');
% title('Raw Measurement');
% 
% subplot(334);
% plot(t_Raw_ave_1,y_raw_1,'b','LineWidth',1.5);
% hold on;
% plot(t_RAW_ave_2,y_raw_2,'k','LineWidth',1.5);
% hold on;
% plot(t_Raw_real_3,y_raw_3,'g','LineWidth',1.5);
% hold on;
% plot(t_Raw_real_4,y_raw_4,'r','LineWidth',1.5);
% 
% ylabel('y_{raw}');
% 
% subplot(337);
% 
% plot(t_Raw_ave_1,theta_raw_1,'b','LineWidth',1.5);
% hold on;
% plot(t_RAW_ave_2,theta_raw_2,'k','LineWidth',1.5);
% hold on;
% plot(t_Raw_real_3,theta_raw_3,'g','LineWidth',1.5);
% hold on;
% plot(t_Raw_real_4,theta_raw_4,'r','LineWidth',1.5);
% xlabel('time');
% ylabel('\theta_{raw}');

% subplot(332);
subplot(321);
plot(t_P_sec_data_1,Pose_X_data_1,'LineWidth',1.9);
Farid Alijani's avatar
Farid Alijani committed
259
hold on
Farid Alijani's avatar
Farid Alijani committed
260
plot(t_P_sec_data_2,Pose_X_data_2,'r','LineWidth',1.8);
Farid Alijani's avatar
Farid Alijani committed
261
hold on
Farid Alijani's avatar
Farid Alijani committed
262
plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.7);
Farid Alijani's avatar
Farid Alijani committed
263
hold on
Farid Alijani's avatar
Farid Alijani committed
264
plot(t_P_sec_data_4,Pose_X_data_4,'m','LineWidth',1.6);
Farid Alijani's avatar
Farid Alijani committed
265
% set(gca,'fontsize',20);
Farid Alijani's avatar
Farid Alijani committed
266
267
268

% ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',20);
% title('a) ${x_{mar}}$','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
269
270
271
% 
% l2 = legend('${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$', '${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.3, I_y = 0.0004, D_y = 0.02}$');
% set(l2,'interpreter','latex','FontSize',9);
Farid Alijani's avatar
Farid Alijani committed
272
% legend('boxoff');
Farid Alijani's avatar
Farid Alijani committed
273

Farid Alijani's avatar
Farid Alijani committed
274
275
276
277
% subplot(335);
subplot(323);

plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',1.9);
Farid Alijani's avatar
Farid Alijani committed
278
hold on
Farid Alijani's avatar
Farid Alijani committed
279
plot(t_P_sec_data_2,Pose_Y_data_2,'r','LineWidth',1.8);
Farid Alijani's avatar
Farid Alijani committed
280
hold on
Farid Alijani's avatar
Farid Alijani committed
281
plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.7);
Farid Alijani's avatar
Farid Alijani committed
282
hold on
Farid Alijani's avatar
Farid Alijani committed
283
plot(t_P_sec_data_4,Pose_Y_data_4,'m','LineWidth',1.6);
Farid Alijani's avatar
Farid Alijani committed
284
% set(gca,'fontsize',20);
Farid Alijani's avatar
Farid Alijani committed
285
286
% ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',20);
% title('b) ${y_{mar}}$','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
287

Farid Alijani's avatar
Farid Alijani committed
288
289
290
291
% subplot(338);
subplot(325);

plot(t_P_sec_data_1,Theta_data_1,'LineWidth',1.9);
Farid Alijani's avatar
Farid Alijani committed
292
hold on
Farid Alijani's avatar
Farid Alijani committed
293
plot(t_P_sec_data_2,Theta_data_2,'r','LineWidth',1.8);
Farid Alijani's avatar
Farid Alijani committed
294
hold on
Farid Alijani's avatar
Farid Alijani committed
295
plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.7);
Farid Alijani's avatar
Farid Alijani committed
296
hold on
Farid Alijani's avatar
Farid Alijani committed
297
plot(t_P_sec_data_4,Theta_data_4,'m','LineWidth',1.6);
Farid Alijani's avatar
Farid Alijani committed
298
% set(gca,'fontsize',20);
Farid Alijani's avatar
Farid Alijani committed
299
300
301
% title('c) ${\theta_{mar}}$','interpreter','latex','FontSize',20);
% xlabel('$ time [s]$','interpreter','latex','FontSize',20);
% ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
302
303


Farid Alijani's avatar
Farid Alijani committed
304
% Control Signals
Farid Alijani's avatar
Farid Alijani committed
305

Farid Alijani's avatar
Farid Alijani committed
306
307
308
% % control signals
% figure;
% set(gcf,'color','white');
Farid Alijani's avatar
Farid Alijani committed
309

Farid Alijani's avatar
Farid Alijani committed
310
311
312
313
% subplot(333);
subplot(322);

plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',1.9);
Farid Alijani's avatar
Farid Alijani committed
314
hold on
Farid Alijani's avatar
Farid Alijani committed
315
plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.8);
Farid Alijani's avatar
Farid Alijani committed
316
hold on
Farid Alijani's avatar
Farid Alijani committed
317
plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.7);
Farid Alijani's avatar
Farid Alijani committed
318
hold on
Farid Alijani's avatar
Farid Alijani committed
319
plot(t_V_sec_data_4,Vel_X_data_4,'m','LineWidth',1.6);
Farid Alijani's avatar
Farid Alijani committed
320
% set(gca,'fontsize',20);
Farid Alijani's avatar
Farid Alijani committed
321
322
% title('d) $\dot{x}_{Rob}$','interpreter','latex','FontSize',20);
% ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
323
324
325
326
327
% 
% l2 = legend('${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$', '${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.3, I_y = 0.0004, D_y = 0.02}$');
% set(l2,'interpreter','latex','FontSize',13);
% legend('boxoff');
axis([0 60 -.01 .17]);
Farid Alijani's avatar
Farid Alijani committed
328

Farid Alijani's avatar
Farid Alijani committed
329
330
331
332
% subplot(336);
subplot(324);

plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',1.9);
Farid Alijani's avatar
Farid Alijani committed
333
hold on
Farid Alijani's avatar
Farid Alijani committed
334
plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.8);
Farid Alijani's avatar
Farid Alijani committed
335
hold on
Farid Alijani's avatar
Farid Alijani committed
336
plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.7);
Farid Alijani's avatar
Farid Alijani committed
337
hold on
Farid Alijani's avatar
Farid Alijani committed
338
plot(t_V_sec_data_4,Vel_Y_data_4,'m','LineWidth',1.6);
Farid Alijani's avatar
Farid Alijani committed
339
% set(gca,'fontsize',20);
Farid Alijani's avatar
Farid Alijani committed
340
341
% ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
% title('e) $\dot{y}_{Rob}$','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
342

Farid Alijani's avatar
Farid Alijani committed
343
344
345
346
% subplot(339);
subplot(326);

plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',1.9);
Farid Alijani's avatar
Farid Alijani committed
347
hold on
Farid Alijani's avatar
Farid Alijani committed
348
plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.8);
Farid Alijani's avatar
Farid Alijani committed
349
hold on
Farid Alijani's avatar
Farid Alijani committed
350
plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.7);
Farid Alijani's avatar
Farid Alijani committed
351
hold on
Farid Alijani's avatar
Farid Alijani committed
352
plot(t_V_sec_data_4,Omega_Z_data_4,'m','LineWidth',1.6);
Farid Alijani's avatar
Farid Alijani committed
353
% set(gca,'fontsize',20);
Farid Alijani's avatar
Farid Alijani committed
354
355
356
% title('f) $\dot{\theta}_{Rob}$','interpreter','latex','FontSize',20);
% xlabel('$ time [s]$','interpreter','latex','FontSize',20);
% ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
357
358

% Trajectory
Farid Alijani's avatar
Farid Alijani committed
359

Farid Alijani's avatar
Farid Alijani committed
360
figure;
Farid Alijani's avatar
Farid Alijani committed
361
362
363
364
365
366
367
set(gcf,'color','white');
subplot(131);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
% hold on
% quiver(abs(Pose_Y_data_1),Pose_X_data_1,gradient(Pose_Y_data_1),gradient(Pose_X_data_1),'r--','LineWidth',.1);

hold on
Farid Alijani's avatar
Farid Alijani committed
368
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
369
370
371
372
373
374
375
376
377
% hold on
% quiver(abs(Pose_Y_data_2),Pose_X_data_2,gradient(Pose_Y_data_2),gradient(Pose_X_data_2),'m--','LineWidth',.1);

hold on
plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
% hold on
% quiver(abs(Pose_Y_data_3),Pose_X_data_3,gradient(Pose_Y_data_3),gradient(Pose_X_data_3),'k--','LineWidth',.1);

hold on
Farid Alijani's avatar
Farid Alijani committed
378
plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
379
380
381
382
% hold on
% quiver(abs(Pose_Y_data_4),Pose_X_data_4,gradient(Pose_Y_data_4),gradient(Pose_X_data_4),'b--','LineWidth',.1);

hold on
Farid Alijani's avatar
Farid Alijani committed
383
384
plot(y_circle,x_circle,'k--','LineWidth',3.4);
% set(gca,'fontsize',20);
Farid Alijani's avatar
Farid Alijani committed
385
386
% title('${a) Approach  + SM  + Target}$','interpreter','latex','FontSize',20);
% ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
387
388
% axis([-.65 .05 -1.6 -.19]);
axis([-.65 .03 -1.47 -.16]);
Farid Alijani's avatar
Farid Alijani committed
389
390
391
subplot(132);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
hold on
Farid Alijani's avatar
Farid Alijani committed
392
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
393
394
395
hold on
plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
hold on
Farid Alijani's avatar
Farid Alijani committed
396
plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
397
hold on
Farid Alijani's avatar
Farid Alijani committed
398
399
plot(y_circle,x_circle,'k--','LineWidth',3.4);
% set(gca,'fontsize',20);
Farid Alijani's avatar
Farid Alijani committed
400
401
% title('$ {b) SM + Target}$','interpreter','latex','FontSize',20);
% xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
402
axis([-.025 0.005 -.29 -.19]);
Farid Alijani's avatar
Farid Alijani committed
403
% axis([-.033 -.01 -.26 -.195]);
Farid Alijani's avatar
Farid Alijani committed
404
405
406
subplot(133);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
hold on
Farid Alijani's avatar
Farid Alijani committed
407
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
408
409
410
hold on
plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
hold on
Farid Alijani's avatar
Farid Alijani committed
411
plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
412
hold on
Farid Alijani's avatar
Farid Alijani committed
413
414
plot(y_circle,x_circle,'k--','LineWidth',3.4);
% set(gca,'fontsize',20);
Farid Alijani's avatar
Farid Alijani committed
415
% title('$ {c) Target}$','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
416
417
% axis([-.0205 -.0168 -.2054 -.199]);
axis([-.0201 -.017 -.206 -.1995]);
Farid Alijani's avatar
Farid Alijani committed
418
419
l = legend('${P_y = .266, I_y = .0003, D_y = .014}$', '${P_y = .28, I_y = 0, D_y = .006}$','${P_y = .29, I_y = .00026, D_y = .015}$','${P_y = .3, I_y = .0004, D_y = .02}$','${Target}$');
set(l,'interpreter','latex','FontSize',12);
Farid Alijani's avatar
Farid Alijani committed
420
legend('boxoff');
Farid Alijani's avatar
Farid Alijani committed
421

Farid Alijani's avatar
Farid Alijani committed
422
423
424
425
426
427
428
429



% no transformation
figure;
set(gcf,'color','white');
plot(y_raw_1,x_raw_1,'b','LineWidth',2);
hold on;
Farid Alijani's avatar
Farid Alijani committed
430
plot(y_raw_2,x_raw_2,'r','LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
431
432
433
hold on;
plot(y_raw_3,x_raw_3,'g','LineWidth',2);
hold on;
Farid Alijani's avatar
Farid Alijani committed
434
plot(y_raw_4,x_raw_4,'m','LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
435
436
437
438


xlabel('y_t');
ylabel('x_t');
Farid Alijani's avatar
Farid Alijani committed
439
% axis([-.1 .1 .19 1.55]);