Commit e9664bcb authored by Farid Alijani's avatar Farid Alijani
Browse files

Low pass filter added

parent a7d879e1
This diff is collapsed.
This diff is collapsed.
clc; clc;
close all; close all;
clear all; clear all;
yaw_off = pi;
%% Import data and time conversion %% Import data and time conversion
% data 1: 556 ros messages for geometry_msgs/Twist % data 1: 556 ros messages for geometry_msgs/Twist
% 557 ros messages for geometry_msgs/PoseStamped % 557 ros messages for geometry_msgs/PoseStamped
% y-axis gains:
% (010816_1718) best!!!
% P = .4; I = 0; D = 0;
% y-axis gains:
% (280716_2024) better!!!
% P = .44; I = 0; D = 0;
% theta-axis gains:
% P = .08; I = 0; D = 0;
% y-axis gains:
% (280716_2002)
% P = .46; I = 0; D = 0;
% theta-axis gains:
% P = .08; I = 0; D = 0;
% Pose_Matrix_data_1 = csvread('POSE_with_Py0.86_Iy0_Dy0.002ANDPt0.08_It0_Dt0.txt',1,0); % Pose_Matrix_data_1 = csvread('POSE_with_Py0.86_Iy0_Dy0.002ANDPt0.08_It0_Dt0.txt',1,0);
% Vel_Matrix_data_1 = csvread('VEL_with_Py0.86_Iy0_Dy0.002ANDPt0.08_It0_Dt0.txt',1,0); % Vel_Matrix_data_1 = csvread('VEL_with_Py0.86_Iy0_Dy0.002ANDPt0.08_It0_Dt0.txt',1,0);
...@@ -91,8 +77,8 @@ t_V_sec_data_2(:,size(t_V_sec_data_2,2)) = []; ...@@ -91,8 +77,8 @@ t_V_sec_data_2(:,size(t_V_sec_data_2,2)) = [];
% Pose_Matrix_data_3 = csvread('POSE_with_Py0.86_Iy0_Dy0.1ANDPt0.08_It0_Dt0.txt',1,0); % Pose_Matrix_data_3 = csvread('POSE_with_Py0.86_Iy0_Dy0.1ANDPt0.08_It0_Dt0.txt',1,0);
% Vel_Matrix_data_3 = csvread('VEL_with_Py0.86_Iy0_Dy0.1ANDPt0.08_It0_Dt0.txt',1,0); % Vel_Matrix_data_3 = csvread('VEL_with_Py0.86_Iy0_Dy0.1ANDPt0.08_It0_Dt0.txt',1,0);
Pose_Matrix_data_3 = csvread('CAM_PID_.3_.0004_.02.txt',1,0); Pose_Matrix_data_3 = csvread('Trans_PID_.28_0_.006.txt',1,0);
Vel_Matrix_data_3 = csvread('vel_PID_.3_.0004_.02.txt',1,0); Vel_Matrix_data_3 = csvread('vel_PID_.28_0_.006.txt',1,0);
% time in position % time in position
TimeP_ros_data_3 = Pose_Matrix_data_3(:,1); % ros time, needs to be converted to sec... TimeP_ros_data_3 = Pose_Matrix_data_3(:,1); % ros time, needs to be converted to sec...
...@@ -124,8 +110,8 @@ t_V_sec_data_3(:,size(t_V_sec_data_3,2)) = []; ...@@ -124,8 +110,8 @@ t_V_sec_data_3(:,size(t_V_sec_data_3,2)) = [];
% Pose_Matrix_data_4 = csvread('POSE_with_Py0.51_Iy0.0005_Dy0.05ANDPt0.08_It0_Dt0.txt',1,0); % Pose_Matrix_data_4 = csvread('POSE_with_Py0.51_Iy0.0005_Dy0.05ANDPt0.08_It0_Dt0.txt',1,0);
% Vel_Matrix_data_4 = csvread('VEL_with_Py0.51_Iy0.0005_Dy0.05ANDPt0.08_It0_Dt0.txt',1,0); % Vel_Matrix_data_4 = csvread('VEL_with_Py0.51_Iy0.0005_Dy0.05ANDPt0.08_It0_Dt0.txt',1,0);
Pose_Matrix_data_4 = csvread('CAM_PID_.39_.0005_.01.txt',1,0); Pose_Matrix_data_4 = csvread('CAM_PID_.266_.0003_.014.txt',1,0);
Vel_Matrix_data_4 = csvread('vel_PID_.39_.0005_.01.txt',1,0); Vel_Matrix_data_4 = csvread('vel_PID_.266_.0003_.014.txt',1,0);
% time in position % time in position
TimeP_ros_data_4 = Pose_Matrix_data_4(:,1); % ros time, needs to be converted to sec... TimeP_ros_data_4 = Pose_Matrix_data_4(:,1); % ros time, needs to be converted to sec...
...@@ -146,13 +132,8 @@ t_V_sec_data_4 = 0:durationV_data_4/size(TimeV_ros_data_4,1):durationV_data_4; ...@@ -146,13 +132,8 @@ 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)) = []; t_V_sec_data_4(:,size(t_V_sec_data_4,2)) = [];
% ----------------------------------------------------------------------------------------------------- % -----------------------------------------------------------------------------------------------------
% data 5: 450 ros messages for geometry_msgs/Twist % data 5: ANDROID CAMERA
% 451 ros messages for geometry_msgs/PoseStamped
% y-axis gains:
% P = .86; I = 0; D = 0.1;
% theta-axis gains:
% P = .08; I = 0; D = 0;
Pose_Matrix_data_5 = csvread('Pose_with_Android.txt',1,0); Pose_Matrix_data_5 = csvread('Pose_with_Android.txt',1,0);
Vel_Matrix_data_5 = csvread('Velocity_with_Android.txt',1,0); Vel_Matrix_data_5 = csvread('Velocity_with_Android.txt',1,0);
...@@ -175,6 +156,32 @@ TimeV_ros_data_5 = Vel_Matrix_data_5(:,1); % ros time, needs to be converted to ...@@ -175,6 +156,32 @@ TimeV_ros_data_5 = Vel_Matrix_data_5(:,1); % ros time, needs to be converted to
t_V_sec_data_5 = 0:durationV_data_5/size(TimeV_ros_data_5,1):durationV_data_5; t_V_sec_data_5 = 0:durationV_data_5/size(TimeV_ros_data_5,1):durationV_data_5;
t_V_sec_data_5(:,size(t_V_sec_data_5,2)) = []; t_V_sec_data_5(:,size(t_V_sec_data_5,2)) = [];
% -----------------------------------------------------------------------------------------------------
% data 6: with several oscilations
Pose_Matrix_data_6 = csvread('CAM_PID_.39_.0005_.01.txt',1,0);
Vel_Matrix_data_6 = csvread('vel_PID_.39_.0005_.01.txt',1,0);
% time in position
TimeP_ros_data_6 = Pose_Matrix_data_6(:,1); % ros time, needs to be converted to sec...
% duriation = end_time - start_time
durationP_data_6 = (TimeP_ros_data_6(size(TimeP_ros_data_6,1),:) - TimeP_ros_data_6(1,:))*10^(-9);
t_P_sec_data_6 = 0:durationP_data_6/size(TimeP_ros_data_6,1):durationP_data_6;
t_P_sec_data_6(:,size(t_P_sec_data_6,2)) = [];
% time in vel
TimeV_ros_data_6 = Vel_Matrix_data_6(:,1); % ros time, needs to be converted to sec...
% duriation = end_time - start_time
durationV_data_6 = (TimeV_ros_data_6(size(TimeV_ros_data_6,1),:) - TimeV_ros_data_6(1,:))*10^(-9);
t_V_sec_data_6 = 0:durationV_data_6/size(TimeV_ros_data_6,1):durationV_data_6;
t_V_sec_data_6(:,size(t_V_sec_data_6,2)) = [];
%% Pose estimation %% Pose estimation
%data 1: %data 1:
...@@ -183,12 +190,14 @@ Pose_X_data_1 = Pose_Matrix_data_1(:,4); ...@@ -183,12 +190,14 @@ Pose_X_data_1 = Pose_Matrix_data_1(:,4);
Pose_Y_data_1 = Pose_Matrix_data_1(:,3); Pose_Y_data_1 = Pose_Matrix_data_1(:,3);
Theta_data_1 = Pose_Matrix_data_1(:,7); Theta_data_1 = Pose_Matrix_data_1(:,7);
% for i = 1:size(Theta_data_1,1)
% when using robot odometry ,,,, % if (Theta_data_1(i) > 0)
% Pose_X = Pose_Matrix(:,2); % Theta_data_1(i) = Theta_data_1(i) - yaw_off;
% Pose_Y = Pose_Matrix(:,3); % else if(Theta_data_1(i) < 0)
% Theta = Pose_Matrix(:,7); % Theta_data_1(i) = Theta_data_1(i) + yaw_off;
% end
% end
% end
% Extracting reference values when the robot is manually docked! % Extracting reference values when the robot is manually docked!
ref_X_data_1 = Pose_X_data_1(size(Pose_X_data_1,1)); ref_X_data_1 = Pose_X_data_1(size(Pose_X_data_1,1));
ref_Y_data_1 = Pose_Y_data_1(size(Pose_Y_data_1,1)); ref_Y_data_1 = Pose_Y_data_1(size(Pose_Y_data_1,1));
...@@ -201,10 +210,14 @@ Pose_X_data_2 = Pose_Matrix_data_2(:,4); ...@@ -201,10 +210,14 @@ Pose_X_data_2 = Pose_Matrix_data_2(:,4);
Pose_Y_data_2 = Pose_Matrix_data_2(:,3); Pose_Y_data_2 = Pose_Matrix_data_2(:,3);
Theta_data_2 = Pose_Matrix_data_2(:,7); Theta_data_2 = Pose_Matrix_data_2(:,7);
% when using robot odometry ,,,, % for i = 1:size(Theta_data_2,1)
% Pose_X = Pose_Matrix(:,2); % if (Theta_data_2(i) > 0)
% Pose_Y = Pose_Matrix(:,3); % Theta_data_2(i) = Theta_data_2(i) - yaw_off;
% Theta = Pose_Matrix(:,7); % else if(Theta_data_2(i) < 0)
% Theta_data_2(i) = Theta_data_2(i) + yaw_off;
% end
% end
% end
% Extracting reference values when the robot is manually docked! % Extracting reference values when the robot is manually docked!
ref_X_data_2 = Pose_X_data_2(size(Pose_X_data_2,1)); ref_X_data_2 = Pose_X_data_2(size(Pose_X_data_2,1));
...@@ -218,11 +231,15 @@ Pose_X_data_3 = Pose_Matrix_data_3(:,4); ...@@ -218,11 +231,15 @@ Pose_X_data_3 = Pose_Matrix_data_3(:,4);
Pose_Y_data_3 = Pose_Matrix_data_3(:,3); Pose_Y_data_3 = Pose_Matrix_data_3(:,3);
Theta_data_3 = Pose_Matrix_data_3(:,7); Theta_data_3 = Pose_Matrix_data_3(:,7);
% when using robot odometry ,,,, %
% Pose_X = Pose_Matrix(:,2); % for i = 1:size(Theta_data_3,1)
% Pose_Y = Pose_Matrix(:,3); % if (Theta_data_3(i) > 0)
% Theta = Pose_Matrix(:,7); % Theta_data_3(i) = Theta_data_3(i) - yaw_off;
% else if(Theta_data_3(i) < 0)
% Theta_data_3(i) = Theta_data_3(i) + yaw_off;
% end
% end
% end
% Extracting reference values when the robot is manually docked! % Extracting reference values when the robot is manually docked!
ref_X_data_3 = Pose_X_data_3(size(Pose_X_data_3,1)); ref_X_data_3 = Pose_X_data_3(size(Pose_X_data_3,1));
ref_Y_data_3 = Pose_Y_data_3(size(Pose_Y_data_3,1)); ref_Y_data_3 = Pose_Y_data_3(size(Pose_Y_data_3,1));
...@@ -232,9 +249,17 @@ ref_Pose_data_3 =[ref_X_data_3;ref_Y_data_3]; ...@@ -232,9 +249,17 @@ ref_Pose_data_3 =[ref_X_data_3;ref_Y_data_3];
%data 4: %data 4:
% when using marker pose ,,,, % when using marker pose ,,,,
Pose_X_data_4 = Pose_Matrix_data_4(:,4); Pose_X_data_4 = Pose_Matrix_data_4(:,4);
Pose_Y_data_4 = abs(Pose_Matrix_data_4(:,3)); Pose_Y_data_4 = Pose_Matrix_data_4(:,3);
Theta_data_4 = -abs(Pose_Matrix_data_4(:,7)); Theta_data_4 = Pose_Matrix_data_4(:,7);
%
% for i = 1:size(Theta_data_4,1)
% if (Theta_data_4(i) > 0)
% Theta_data_4(i) = Theta_data_4(i) - yaw_off;
% else if(Theta_data_4(i) < 0)
% Theta_data_4(i) = Theta_data_4(i) + yaw_off;
% end
% end
% end
% Extracting reference values when the robot is manually docked! % Extracting reference values when the robot is manually docked!
ref_X_data_4 = Pose_X_data_4(size(Pose_X_data_4,1)); ref_X_data_4 = Pose_X_data_4(size(Pose_X_data_4,1));
...@@ -255,17 +280,27 @@ ref_Y_data_5 = Pose_Y_data_5(size(Pose_Y_data_5,1)); ...@@ -255,17 +280,27 @@ ref_Y_data_5 = Pose_Y_data_5(size(Pose_Y_data_5,1));
ref_Theta_data_5 = Theta_data_5(size(Theta_data_5,1)); ref_Theta_data_5 = Theta_data_5(size(Theta_data_5,1));
ref_Pose_data_5 =[ref_X_data_5;ref_Y_data_5]; ref_Pose_data_5 =[ref_X_data_5;ref_Y_data_5];
%data 6:
% when using marker pose ,,,,
Pose_X_data_6 = Pose_Matrix_data_6(:,4);
Pose_Y_data_6 = Pose_Matrix_data_6(:,3);
Theta_data_6 = Pose_Matrix_data_6(:,7);
% Extracting reference values when the robot is manually docked!
ref_X_data_6 = Pose_X_data_6(size(Pose_X_data_6,1));
ref_Y_data_6 = Pose_Y_data_6(size(Pose_Y_data_6,1));
ref_Theta_data_6 = Theta_data_6(size(Theta_data_6,1));
ref_Pose_data_6 =[ref_X_data_6;ref_Y_data_6];
% Reference Circle % Reference Circle
thresh_X = .001; thresh_X = .001;
theta = 0:.001:2*pi; theta = 0:.001:2*pi;
% needs to be adjusted manually if docking platform is replaced! % needs to be adjusted manually if docking platform is replaced!
ref_x = .20095; ref_x = .2026;
ref_y = .0081; ref_y = .0087;
x_circle = thresh_X*cos(theta) + ref_x; % ref_X needs to be recorded 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 y_circle = thresh_X*sin(theta) + ref_y; % ref_Y needs to be recorded
%% Velocity estimation %% Velocity estimation
% data 1: % data 1:
...@@ -293,16 +328,21 @@ Vel_X_data_5 = Vel_Matrix_data_5(:,2); ...@@ -293,16 +328,21 @@ Vel_X_data_5 = Vel_Matrix_data_5(:,2);
Vel_Y_data_5 = Vel_Matrix_data_5(:,3); Vel_Y_data_5 = Vel_Matrix_data_5(:,3);
Omega_Z_data_5 = Vel_Matrix_data_5(:,7); Omega_Z_data_5 = Vel_Matrix_data_5(:,7);
% data 6:
Vel_X_data_6 = Vel_Matrix_data_6(:,2);
Vel_Y_data_6 = Vel_Matrix_data_6(:,3);
Omega_Z_data_6 = Vel_Matrix_data_6(:,7);
%% Plots %% Plots
% marker position % marker position
figure; figure;
plot(t_P_sec_data_4,Pose_Y_data_4,'k','LineWidth',1.6); plot(t_P_sec_data_6,Pose_Y_data_6,'k','LineWidth',1.6);
xlabel('time'); xlabel('time');
ylabel('y'); ylabel('y');
figure; figure;
plot(t_P_sec_data_4,Theta_data_4,'k','LineWidth',1.6); plot(t_P_sec_data_6,Theta_data_6,'k','LineWidth',1.6);
xlabel('time'); xlabel('time');
ylabel('\theta'); ylabel('\theta');
...@@ -310,38 +350,39 @@ ylabel('\theta'); ...@@ -310,38 +350,39 @@ ylabel('\theta');
figure; figure;
set(gcf,'color','white'); set(gcf,'color','white');
subplot(3,1,1); subplot(3,1,1);
plot(t_P_sec_data_1,Pose_X_data_1,'LineWidth',3); plot(t_P_sec_data_1,Pose_X_data_1,'LineWidth',2.1);
hold on hold on
plot(t_P_sec_data_2,Pose_X_data_2,'r','LineWidth',1.7); plot(t_P_sec_data_2,Pose_X_data_2,'r','LineWidth',1.7);
hold on hold on
plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',2.3); plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.8);
% hold on hold on
% plot(t_P_sec_data_4,Pose_X_data_4,'m','LineWidth',1.6); plot(t_P_sec_data_4,Pose_X_data_4,'m','LineWidth',1.5);
title('Pose estimation in marker coordinate system'); title('Pose estimation in marker coordinate system');
ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',14); ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',14);
l2 = legend('${P_y = 0.28, I_y = 0.0001, D_y = 0.01}$', '${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.3, I_y = 0.0004, D_y = 0.02}$','${P_y = 0.39, I_y = 0.0005, D_y = 0.01}$'); l2 = legend('${P_y = 0.28, I_y = 0.0001, D_y = 0.01}$', '${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$');
set(l2,'interpreter','latex','FontSize',11); set(l2,'interpreter','latex','FontSize',11);
legend('boxoff'); legend('boxoff');
subplot(3,1,2); subplot(3,1,2);
plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',3); plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',2.1);
hold on hold on
plot(t_P_sec_data_2,Pose_Y_data_2,'r','LineWidth',1.7); plot(t_P_sec_data_2,Pose_Y_data_2,'r','LineWidth',1.7);
hold on hold on
plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',2.3); plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.8);
% hold on hold on
% plot(t_P_sec_data_4,Pose_Y_data_4,'m','LineWidth',1.6); plot(t_P_sec_data_4,Pose_Y_data_4,'m','LineWidth',1.5);
ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',14); ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',14);
% axis([0 80 -.1 .67])
subplot(3,1,3); subplot(3,1,3);
plot(t_P_sec_data_1,Theta_data_1,'LineWidth',3); plot(t_P_sec_data_1,Theta_data_1,'LineWidth',2.1);
hold on hold on
plot(t_P_sec_data_2,Theta_data_2,'r','LineWidth',1.7); plot(t_P_sec_data_2,Theta_data_2,'r','LineWidth',1.7);
hold on hold on
plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',2.3); plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.8);
% hold on hold on
% plot(t_P_sec_data_4,Theta_data_4,'m','LineWidth',1.6); plot(t_P_sec_data_4,Theta_data_4,'m','LineWidth',1.5);
xlabel('$ time [sec]$','interpreter','latex','FontSize',14); xlabel('$ time [sec]$','interpreter','latex','FontSize',14);
ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',14); ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',14);
...@@ -351,41 +392,40 @@ figure; ...@@ -351,41 +392,40 @@ figure;
set(gcf,'color','white'); set(gcf,'color','white');
subplot(3,1,1); subplot(3,1,1);
plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',3); plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',2.1);
hold on hold on
plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.7); plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.7);
hold on hold on
plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',2.3); plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.8);
% hold on hold on
% plot(t_V_sec_data_4,Vel_X_data_4,'m','LineWidth',1.6); plot(t_V_sec_data_4,Vel_X_data_4,'m','LineWidth',1.5);
title('Control Signals'); title('Control Signals');
ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',14); ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
axis([0 45 -.01 .2]);
l2 = legend('${P_y = 0.28, I_y = 0.0001, D_y = 0.01}$', '${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.3, I_y = 0.0004, D_y = 0.02}$','${P_y = 0.39, I_y = 0.0005, D_y = 0.01}$'); l2 = legend('${P_y = 0.28, I_y = 0.0001, D_y = 0.01}$', '${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$');
set(l2,'interpreter','latex','FontSize',11); set(l2,'interpreter','latex','FontSize',11);
legend('boxoff'); legend('boxoff');
subplot(3,1,2); subplot(3,1,2);
plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',3); plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',2.1);
hold on hold on
plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.7); plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.7);
hold on hold on
plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',2.3); plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.8);
% hold on hold on
% plot(t_V_sec_data_4,Vel_Y_data_4,'m','LineWidth',1.6); plot(t_V_sec_data_4,Vel_Y_data_4,'m','LineWidth',1.5);
ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',14); ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
subplot(3,1,3); subplot(3,1,3);
plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',3); plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',2.1);
hold on hold on
plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.7); plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.7);
hold on hold on
plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',2.3); plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.8);
% hold on hold on
% plot(t_V_sec_data_4,Omega_Z_data_4,'m','LineWidth',1.6); plot(t_V_sec_data_4,Omega_Z_data_4,'m','LineWidth',1.5);
xlabel('$ time [sec]$','interpreter','latex','FontSize',14); xlabel('$ time [sec]$','interpreter','latex','FontSize',14);
ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14); ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
...@@ -420,8 +460,8 @@ ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14); ...@@ -420,8 +460,8 @@ ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
% quiver(Pose_Y_data_1,Pose_X_data_1,Vy,Vx,'r--','LineWidth',.1); % quiver(Pose_Y_data_1,Pose_X_data_1,Vy,Vx,'r--','LineWidth',.1);
figure; figure;
plot(Pose_Y_data_4,Pose_X_data_4,'k','LineWidth',1.1); plot(Pose_Y_data_6,Pose_X_data_6,'k','LineWidth',1.4);
axis([-.5 .7 .1 1.6]); axis([-.3 .7 .1 1.6]);
figure; figure;
...@@ -440,8 +480,8 @@ plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2); ...@@ -440,8 +480,8 @@ plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
% hold on % 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); % 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 hold on
% plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2); plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
% hold on % 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); % quiver(abs(Pose_Y_data_4),Pose_X_data_4,gradient(Pose_Y_data_4),gradient(Pose_X_data_4),'b--','LineWidth',.1);
...@@ -450,8 +490,7 @@ plot(y_circle,x_circle,'k--','LineWidth',3.4); ...@@ -450,8 +490,7 @@ plot(y_circle,x_circle,'k--','LineWidth',3.4);
title('Approach zone + SM zone + Target area'); title('Approach zone + SM zone + Target area');
ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',14); ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',14);
% axis([-.1 .4 .18 1.4]); axis([-.05 .26 .19 1.52]);
axis([-.5 .7 .1 1.6])
subplot(1,3,2); subplot(1,3,2);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2); plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
...@@ -459,14 +498,14 @@ hold on ...@@ -459,14 +498,14 @@ hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2); plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
hold on hold on
plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2); plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
% hold on hold on
% plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2); plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
hold on hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4); plot(y_circle,x_circle,'k--','LineWidth',3.4);
title('SM zone + Target area'); title('SM zone + Target area');
xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14); xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14);
% axis([-.005 0.045 .195 .32]); axis([-.023 0.02 .19 .29]);
subplot(1,3,3); subplot(1,3,3);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2); plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
...@@ -474,15 +513,15 @@ hold on ...@@ -474,15 +513,15 @@ hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2); plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
hold on hold on
plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2); plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
% hold on hold on
% plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2); plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
hold on hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4); plot(y_circle,x_circle,'k--','LineWidth',3.4);
title('Target area'); title('Target area');
% axis([0.0065 .0095 .1995 .2055]); axis([0.0072 .0101 .20 .2056]);
l = legend('${P_y = 0.28, I_y = 0.0001, D_y = 0.01}$', '${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.3, I_y = 0.0004, D_y = 0.02}$','${P_y = 0.39, I_y = 0.0005, D_y = 0.01}$'); l = legend('${P_y = 0.28, I_y = 0.0001, D_y = 0.01}$', '${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$','${Target}$');
set(l,'interpreter','latex','FontSize',11); set(l,'interpreter','latex','FontSize',11);
% legend('boxoff'); % legend('boxoff');
......
%time,field.linear.x,field.linear.y,field.linear.z,field.angular.x,field.angular.y,field.angular.z
1475142133471721490,0.15,-0.155687968624,0.0,0.0,0.0,0.0262040376663
1475142133570973126,0.15,-0.128301917254,0.0,0.0,0.0,0.0262552976608
1475142133659495245,0.15,-0.132192679193,0.0,0.0,0.0,0.0285806536674
1475142133746193076,0.15,-0.127596980849,0.0,0.0,0.0,0.0262552976608
1475142133826791404,0.15,-0.128286335601,0.0,0.0,0.0,0.0262552976608
1475142133912415561,0.15,-0.128286335601,0.0,0.0,0.0,0.0262552976608
1475142133997050948,0.15,-0.128286335601,0.0,0.0,0.0,0.0262552976608
1475142134083427473,0.15,-0.128286335601,0.0,0.0,0.0,0.0262552976608
1475142134168200746,0.15,-0.139931824612,0.0,0.0,0.0,0.0330161929131
1475142134249984718,0.15,-0.143723731642,0.0,0.0,0.0,0.03640614748
1475142134332259419,0.15,0.23602881493,0.0,0.0,0.0,-0.159193885326
1475142134416533589,0.15,-0.213454988798,0.0,0.0,0.0,0.0412234544754
1475142134498776413,0.15,-0.113154391044,0.0,0.0,0.0,0.0250136733055
1475142134579934462,0.15,-0.138385270116,0.0,0.0,0.0,0.0376436591148
1475142134657142335,0.15,-0.133910724266,0.0,0.0,0.0,0.0394171953201
1475142134747776448,0.15,-0.102357944053,0.0,0.0,0.0,0.0226619124413
1475142134826155759,0.15,-0.144368457629,0.0,0.0,0.0,0.0446200251579
1475142134907310986,0.15,-0.120751795626,0.0,0.0,0.0,0.0371652722359
1475142134988198593,0.15,0.17820376565,0.0,0.0,0.0,-0.129774224758
1475142135072319109,0.15,-0.195723697929,0.0,0.0,0.0,0.0539032101631
1475142135154992991,0.15,-0.120422777599,0.0,0.0,0.0,0.0443633079529
1475142135252819833,0.15,-0.105699020029,0.0,0.0,0.0,0.0356554865837
<