From 423a424be6c20f993f5b9c212128738f053ce927 Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker@tutanota.com>
Date: Sun, 6 Oct 2024 14:54:24 +0200
Subject: [PATCH] use active set algorithm to workaround singular hessian

---
 control_act.m | 32 +++++++++++++++++++++++---------
 tesi.m        |  4 +++-
 2 files changed, 26 insertions(+), 10 deletions(-)

diff --git a/control_act.m b/control_act.m
index 4cafb1b..ecbb4a9 100644
--- a/control_act.m
+++ b/control_act.m
@@ -30,13 +30,17 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
     if eq(pred_hor, 0)
         return
     elseif eq(pred_hor, 1)
-        % minimize wcorr_r^2 + wcorr_l^2
-        %H = eye(2);
 
+        if eq(sim_data.costfun, 1)
+        % minimize vcorr_r^2 + wcorr_l^2
+        H = eye(2);
+        elseif eq(sim_data.costfun, 2)
         % ex1: minimize v=r(wr+wl)/2
-        %H = sim_data.r*sim_data.r*0.5*ones(2,2);
+        H = sim_data.r*sim_data.r*0.5*ones(2,2);
+        elseif eq(sim_data.costfun, 3)
         % ex2: minimize w=r(wr-wl)/d
         H = sim_data.r*sim_data.r*2*[1, -1; -1, 1]/(sim_data.d*sim_data.d);
+        end
 
         f = zeros(2,1);
         T_inv = decouple_matrix(q_act, sim_data);
@@ -45,8 +49,8 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
         d = T_inv*ut;
         
         % solve qp problem
-        options = optimoptions('quadprog', 'Display', 'off');
-        u_corr = quadprog(H, f, [], [], [],[], -s_ - d, s_-d, [], options);
+        options = optimoptions('quadprog', 'Algorithm','active-set','Display','off');
+        u_corr = quadprog(H, f, [], [], [],[], -s_ - d, s_-d, zeros(2,1), options);
         
         q_pred = q_act;
         U_corr_history(:,:,1) = u_corr;
@@ -127,16 +131,26 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
             lb = [lb; -s_-d];
             ub = [ub; s_-d];
         end
-       
+
+        if eq(sim_data.costfun, 1)
+        % minimize vcorr_r^2 + wcorr_l^2
         % squared norm of u_corr. H must be identity,
         H = eye(pred_hor*2)*2;
+        elseif eq(sim_data.costfun, 2)
+        % ex1: minimize v=r(wr+wl)/2
+        H = kron(eye(pred_hor), sim_data.r*sim_data.r*0.5*ones(2,2));
+        elseif eq(sim_data.costfun, 3)
+        % ex2: minimize w=r(wr-wl)/d
+        H = kron(eye(pred_hor), sim_data.r*sim_data.r*2*[1, -1; -1, 1]/(sim_data.d*sim_data.d));
+        end
+       
+        
         % no linear terms
         f = zeros(pred_hor*2, 1);
     
         % solve qp problem
-        options = optimoptions('quadprog', 'Display', 'off');
-        U_corr = quadprog(H, f, [], [], [],[],lb,ub,[],options);
-    
+        options = optimoptions('quadprog', 'Algorithm','active-set','Display','off');
+        U_corr = quadprog(H, f, [], [], [],[], lb, ub, zeros(2*pred_hor,1), options);    
         % reshape the vector of vectors to be an array, each element being
         % u_corr_j as a 2x1 vector
         % and add the prediction at t_k+C
diff --git a/tesi.m b/tesi.m
index 182ce85..4cc9254 100644
--- a/tesi.m
+++ b/tesi.m
@@ -29,12 +29,14 @@ for i = 1:length(TESTS)
     [ref dref] = set_trajectory(sim_data.TRAJECTORY, sim_data);
     sim_data.ref = ref;
     sim_data.dref = dref;
+    sim_data.costfun=2
+    sim_data.tc=0.05;
     
     % spawn a new worker for each controller
     % 1: track only
     % 2: track + 1step
     % 3: track + multistep
-    spmd (2)
+    spmd (3)
         worker_index = spmdIndex;
         % load controller-specific options
         data = load(['tests/' num2str(worker_index) '.mat']);