also plot trajectory and error relative to output position

control_act.m

@@ -118,23 +118,15 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
         % A will be at most PREDICTION_HORIZON * 2 * 2 (2: size of T_inv; 2:
         % accounting for T_inv and -T_inv) by PREDICTION_HORIZON (number of
         % vectors in u_corr times the number of elements [2] in each vector)
-        A_deq = [];
         b_deq = [];
-    
-        s_ = SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE;
         for k=1:pred_hor
             T_inv = T_inv_pred(:,:,k);
             u_track = u_track_pred(:,:,k);
-    
-            % [T_inv; -T_inv] is a 4x2 matrix
-            %A_deq = blkdiag(A_deq, [T_inv; -T_inv]);
-            A_deq = blkdiag(A_deq, [eye(2); -eye(2)]);
-
-    
             d = T_inv*u_track;
             b_deq = [b_deq; s_ - d; s_ + d];
         end
         
+        A_deq = kron(eye(pred_hor), [eye(2); -eye(2)]);
         %A_deq
         %b_deq
         % unknowns

plot_all.m

@@ -8,7 +8,6 @@ disp('Photos will start in 3s')
 pause(3)
 
 PLOT_TESTS = [
-    %{
     "results-diffdrive/straightline/chill/11-09-2024-16-57-01";
     "results-diffdrive/straightline/chill_errortheta_pisixths/11-09-2024-16-57-43";
     "results-diffdrive/straightline/chill_errory/11-09-2024-16-59-04";
@@ -18,17 +17,16 @@ PLOT_TESTS = [
     "results-diffdrive/figure8/chill/11-09-2024-17-00-53";
     %"results-diffdrive/figure8/fancyreps/11-09-2024--45-28";
     "results-diffdrive/figure8/toofast/11-09-2024-17-01-43";
-    %}
 
-    %"results-unicycle/straightline/chill/11-09-2024-17-07-51";
+    "results-unicycle/straightline/chill/11-09-2024-17-07-51";
-    %"results-unicycle/straightline/chill_errortheta_pisixths/11-09-2024-17-08-35";
+    "results-unicycle/straightline/chill_errortheta_pisixths/11-09-2024-17-08-35";
-    %"results-unicycle/straightline/chill_errory/11-09-2024-17-10-00";
+    "results-unicycle/straightline/chill_errory/11-09-2024-17-10-00";
-    %"results-unicycle/straightline/toofast/11-09-2024-17-09-18";
+    "results-unicycle/straightline/toofast/11-09-2024-17-09-18";
-    %"results-unicycle/circle/start_center/11-09-2024-17-10-48";
+    "results-unicycle/circle/start_center/11-09-2024-17-10-48";
     "results-unicycle/square/11-09-2024-17-17-21";
-    %"results-unicycle/figure8/chill/11-09-2024-17-11-53";
+    "results-unicycle/figure8/chill/11-09-2024-17-11-53";
     %"results-unicycle/figure8/fancyreps/11-09-2024--45-28";
-    %"results-unicycle/figure8/toofast/11-09-2024-17-12-45";
+    "results-unicycle/figure8/toofast/11-09-2024-17-12-45";
     ]
 
 s_ = size(PLOT_TESTS)
@@ -60,7 +58,7 @@ for i = 1:s_(1)
         pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_triple_input_1x2.png'])
         pause(1); clf; plot_tripleinput(t{n}, sim_data{n}.SATURATION, U{n}, U_track{n}, U_corr{n}, 1, in1, in2, m1, m2)
         pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_triple_input_2x1.png'])
-        
+       
         %pause(1); clf; plot_input(t{n}, sim_data{n}.SATURATION, U_track{n}, 'track')
         %pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_track_input.png'])
         %pause(1); clf; plot_input(t{n}, sim_data{n}.SATURATION, U_corr{n}, 'corr')
@@ -70,6 +68,11 @@ for i = 1:s_(1)
         pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_total_input_1x2.png'])
         pause(1); clf; plot_input(t{n},sim_data{n}.SATURATION,  U{n}, 1, '', in1, in2, m1, m2)
         pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_total_input_2x1.png'])
+
+        pause(1); clf; plot_trajectory(t{n}, ref_t{n}, y{n})
+        pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_trajectory_out.png'])
+        pause(1); clf; plot_error(t{n}, ref_t{n}, y{n})
+        pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_error_out.png'])
     end
     
     % correction difference (multistep, 1-step)
@@ -95,5 +98,4 @@ for i = 1:s_(1)
     pause(0.5); export_fig(gcf, '-transparent', [dir, 'input_diff_1step_multistep_1x2.png'])
     pause(1); clf; plot_input_diff(t{2}, t{3}, U{2}, U{3}, 1, ['\textbf{$$' in1 '^{1step}$$}'], ['\textbf{$$' in1 '^{multistep}$$}'], ['\textbf{$$' in2 '^{1step}$$}'], ['\textbf{$$' in2 '^{multistep}$$}'], m1, m2)
     pause(0.5); export_fig(gcf, '-transparent', [dir, 'input_diff_1step_multistep_2x1.png'])
-
 end

tesi.m

@@ -149,7 +149,7 @@ function [t, q, y, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = sim
         
         y1 = q(:, 1) + sim_data.b * cos(q(:,3));
         y2 = q(:, 2) + sim_data.b * sin(q(:,3));
-        y = [y; y1, y2];
+        y = [y; [y1, y2]];
     end
 
     ref_t = double(subs(sim_data.ref, t'))';