From ceb7659bcc61b9ba4c24d7cca052f35b94a6d199 Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker@tutanota.com>
Date: Fri, 13 Sep 2024 20:26:01 +0200
Subject: [PATCH] misc tests

---
 control_act.m | 16 ++++++++--------
 disaster.m    |  6 ++----
 2 files changed, 10 insertions(+), 12 deletions(-)

diff --git a/control_act.m b/control_act.m
index 8f9ec21..a62e3b2 100644
--- a/control_act.m
+++ b/control_act.m
@@ -1,11 +1,9 @@
 function [u, ut, uc, U_corr_history, q_pred] = control_act(t, q, sim_data)   
     dc = decouple_matrix(q, sim_data);
     ut = utrack(t, q, sim_data);
-    [uc, U_corr_history, q_pred] = ucorr(t, q, sim_data);
-
     ut = dc*ut;
-    %uc = dc*uc;
-    %uc = zeros(2,1);
+
+    [uc, U_corr_history, q_pred] = ucorr(t, q, sim_data);
 
     u = ut+uc;
     % saturation
@@ -30,7 +28,8 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
     if eq(pred_hor, 0)
         return
     elseif eq(pred_hor, 1)
-        H = eye(2);
+        H = [1, 0; 0, -1];
+        %H = eye(2);
         f = zeros(2,1);
         T_inv = decouple_matrix(q_act, sim_data);
         ut = utrack(t, q_act, sim_data);
@@ -41,8 +40,8 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
         b = [s_-d;s_+d];
         
         % solve qp problem
-        options = optimoptions('quadprog', 'Display', 'off');
-        u_corr = quadprog(H, f, A, b, [],[],[],[],[],options);
+        options = optimoptions('quadprog');
+        u_corr = quadprog(H, f, A, b, [],[],[],[],[],options)
         
         q_pred = q_act;
         U_corr_history(:,:,1) = u_corr;
@@ -133,7 +132,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 = eye(pred_hor*2)*2;
+        H = kron(eye(pred_hor), [1,0;0,0]);
         % no linear terms
         f = zeros(pred_hor*2, 1);
     
diff --git a/disaster.m b/disaster.m
index 689bb2e..67b4d48 100644
--- a/disaster.m
+++ b/disaster.m
@@ -1,13 +1,11 @@
 clc
 clear all
-close all
+close allQQQ
 
 %load('results-diffdrive/circle/start_center/10-09-2024 13-27-12/workspace_composite.mat')
-%load('results-diffdrive/circle/start_center/10-09-2024 15-33-08/workspace_composite.mat')
+load('results-diffdrive/circle/start_center/10-09-2024 15-33-08/workspace_composite.mat')
 %load('/home/emamaker/documents/Università/tesi/tesi-sim/results-diffdrive/square/10-09-2024 13-53-35/workspace_composite.mat')
 
-load('results-diffdrive/figure8/toofast/10-09-2024-22-35-17/workspace_composite.mat')
-
 
 
 y = cell(1,3);