plot trajectory in the xy plane

set_trajectory.m

@@ -11,14 +11,22 @@ switch i
         xref = 10*s;
         yref = 0;
     case 2
+        % straight line, with initial error
+        xref = 5 + 0.5*s;
+        yref = 0;
+    case 3
+        % straight line, initial error, faster
+        xref = 5 + 10*s;
+        yref = 0;
+    case 4
         xref = 5*cos(s);
         yref = 5*sin(s);
-    case 3
+    case 5
         xref = 15*cos(s);
         yref = 15*sin(s);
-    case 4
-        xref = 5*cos(0.05*s)
-        yref = 5*cos(0.05*s)
+    case 6
+        xref = 5*cos(0.05*s);
+        yref = 5*sin(0.05*s);
 end
 
 ref = [xref; yref];

tesiema.m

@@ -4,7 +4,7 @@ close all
 
 global x0 ref dref b K saturation
 
-TRAJECTORY = 4
+TRAJECTORY = 1
 INITIAL_CONDITIONS = 0
 % distance from the center of the unicycle to the point being tracked
 % ATTENZIONE! CI SARA' SEMPRE UN ERRORE COSTANTE DOVUTO A b. Minore b,
@@ -13,7 +13,7 @@ b = 0.2
 % proportional gain
 K = eye(2)*2.5
 % saturation
-saturation = [5; 0.5];
+saturation = [5; 0.2];
 
 
 % initial state
@@ -39,41 +39,47 @@ end
 % plot results
 ref_t = double(subs(ref, t'))';
 
-subplot(3,2,1)
+subplot(2,2,1)
+hold on
+plot(ref_t(:, 1), ref_t(:, 2), "DisplayName", "Ref")
+plot(x(:, 1), x(:, 2), "DisplayName", "state")
+xlabel('x')
+ylabel('y')
+legend()
+subplot(2,2,3)
+plot(t, U(:, 1))
+xlabel('t')
+ylabel('input v')
+subplot(2,2,4)
+plot(t, U(:, 2))
+xlabel('t')
+ylabel('input w')
+
+
+subplot(4,4,3)
 hold on
 xlabel('t')
 ylabel('x')
-plot(t, ref_t(:, 1));
-plot(t, x(:, 1));
+plot(t, ref_t(:, 1), "DisplayName", "X_{ref}");
+plot(t, x(:, 1), "DisplayName", "X");
 legend()
 hold off
 
-subplot(3,2,2)
+subplot(4,4,4)
 plot(t, ref_t(:, 1) - x(:, 1));
 xlabel('t')
 ylabel('x error')
 
-subplot(3,2,3)
+subplot(4,4,7)
 hold on
 xlabel('t')
 ylabel('y')
-plot(t, ref_t(:, 2));
-plot(t, x(:, 2));
+plot(t, ref_t(:, 2), "DisplayName", "Y_{ref}");
+plot(t, x(:, 2), "DisplayName", "Y");
 legend()
 hold off
 
-subplot(3,2,4)
+subplot(4,4,8)
 plot(t, ref_t(:, 2) - x(:, 2));
 xlabel('t')
 ylabel('y error')
-
-
-subplot(3,2,5)
-plot(t, U(:, 1))
-xlabel('t')
-ylabel('input v')
-
-subplot(3,2,6)
-plot(t, U(:, 2))
-xlabel('t')
-ylabel('input w')