From cceb2f1379cd80532c6ef4b9392b8eab3f620a9f Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker@tutanota.com>
Date: Mon, 16 Sep 2024 18:27:41 +0200
Subject: [PATCH] minimize vcorr^2 on correction before decoupling

---
 control_act.m | 13 +++++--------
 tesi.m        |  2 +-
 2 files changed, 6 insertions(+), 9 deletions(-)

diff --git a/control_act.m b/control_act.m
index b0f17a3..1ff786b 100644
--- a/control_act.m
+++ b/control_act.m
@@ -31,9 +31,10 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
     elseif eq(pred_hor, 1)
         T_inv = decouple_matrix(q_act, sim_data);
         ut = utrack(t, q_act, sim_data);
+        
+        % minimize v
+        H = 2*eye(2);
 
-        H = 2 * (T_inv') * T_inv;
-        %H = eye(2);
         f = zeros(2,1);
         A = [T_inv; -T_inv];        
         %A = [eye(2); -eye(2)];
@@ -121,19 +122,14 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
         % vectors in u_corr times the number of elements [2] in each vector)
         A_deq = [];
         b_deq = [];
-        H1 = [];
         for k=1:pred_hor
             T_inv = T_inv_pred(:,:,k);
             u_track = u_track_pred(:,:,k);
             d = T_inv*u_track;
 
-            H1 = blkdiag(H1, T_inv);
-            H2 = blkdiag(H2, T_inv');
             A_deq = blkdiag(A_deq, [T_inv; -T_inv]);
             b_deq = [b_deq; s_ - d; s_ + d];
         end
-
-        H = H1'*H1;
         
         %A_deq = kron(eye(pred_hor), [eye(2); -eye(2)]);
         %A_deq
@@ -143,7 +139,8 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
         % squared norm of u_corr. H must be identity,
         % PREDICTION_HORIZON*size(u_corr)
         %H = eye(pred_hor*2)*2;
-        %H = kron(eye(pred_hor), [1,0;0,0]);
+        H = kron(eye(pred_hor), 2*eye(2));
+        %H = kron(eye(pred_hor), 2*ones(2,2));
         % no linear terms
         f = zeros(pred_hor*2, 1);
     
diff --git a/tesi.m b/tesi.m
index 27483a9..54c551b 100644
--- a/tesi.m
+++ b/tesi.m
@@ -8,6 +8,7 @@ ROBOT = 'diffdrive'
 TESTS = ["circle/start_center"]
 
 % main
+
 s_ = size(TESTS);
 for i = 1:length(TESTS)
     clearvars -except i s_ TESTS ROBOT
@@ -24,7 +25,6 @@ for i = 1:length(TESTS)
         sim_data.(fn{1}) = test_data.(fn{1});
     end
     
-    sim_data.r = 0.06
     % set trajectory and starting conditions
     sim_data.q0 = set_initial_conditions(sim_data.INITIAL_CONDITIONS);
     [ref dref] = set_trajectory(sim_data.TRAJECTORY, sim_data);