From 44f65aed779d0f6a794e841c339d41abf677748e Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker@tutanota.com>
Date: Tue, 16 Jul 2024 10:58:00 +0200
Subject: [PATCH] 1-step mpc

---
 control_act.m    | 52 +++++++++++++++++++++++++++++++++++++++++++-----
 set_trajectory.m |  8 ++++----
 tesiema.m        | 44 ++++++++++++++++++++++++++++++++--------
 3 files changed, 87 insertions(+), 17 deletions(-)

diff --git a/control_act.m b/control_act.m
index cfdf823..fef1262 100644
--- a/control_act.m
+++ b/control_act.m
@@ -1,22 +1,64 @@
 function u = control_act(t, x)
-
-    global ref dref K b saturation
+    global ref dref K b SATURATION PREDICTION_SATURATION_TOLERANCE USE_PREDICTION PREDICTION_STEPS
 
     ref_s = double(subs(ref, t));
     dref_s = double(subs(dref, t));
 
     
     err = ref_s - feedback(x);
-    u_nom = dref_s + K*err;
+    u_track = dref_s + K*err;
     
     theta = x(3);
 
     T_inv = [cos(theta), sin(theta); -sin(theta)/b, cos(theta)/b];
+    
+    u = zeros(2,1);
+    if USE_PREDICTION==true 
+        % 1-step prediction
 
-    u = T_inv * ( u_nom );
+        % quadprog solves the problem in the form
+        % min 1/2 x'Hx +f'x
+        % where x is u_corr. Since u_corr is (v_corr; w_corr), and I want
+        % to minimize u'u (norm squared of the function itself) H must be
+        % the identity matrix of size 2
+        H = eye(2)*2;
+        % no linear of constant terms, so 
+        f = [];
+
+        % and there are box constraints on the saturation, as upper/lower
+        % bounds
+        %T = inv(T_inv);
+        %lb = -T*saturation - u_track;
+        %ub = T*saturation - u_track;
+        % matlab says this is a more efficient way of doing
+        % inv(T_inv)*saturation
+        %lb = -T_inv\saturation - u_track;
+        %ub = T_inv\saturation - u_track;
+
+        % Resolve box constraints as two inequalities
+        A_deq = [T_inv; -T_inv];
+        d = T_inv*u_track;
+        b_deq = [SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE - d;
+            SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE + d];
+        
+        % solve the problem
+        % no <= constraints
+        % no equality constraints
+        % only upper/lower bound constraints        
+        options = optimoptions('quadprog', 'Display', 'off');
+        u_corr = quadprog(H, f, A_deq, b_deq, [],[],[],[],[],options);
+
+        u = T_inv * (u_track + u_corr);
+
+        global tu uu
+        tu = [tu, t];
+        uu = [uu, u_corr];
+    else
+        u = T_inv * u_track;
+    end
 
     % saturation
-    u = min(saturation, max(-saturation, u));
+    u = min(SATURATION, max(-SATURATION, u));
 end
 
 function x_track = feedback(x)
diff --git a/set_trajectory.m b/set_trajectory.m
index d8ce540..a218fe4 100644
--- a/set_trajectory.m
+++ b/set_trajectory.m
@@ -19,14 +19,14 @@ switch i
         xref = 5 + 10*s;
         yref = 0;
     case 4
-        xref = 5*cos(s);
-        yref = 5*sin(s);
+        xref = 2.5*cos(s);
+        yref = 2.5*sin(s);
     case 5
         xref = 15*cos(s);
         yref = 15*sin(s);
     case 6
-        xref = 5*cos(0.05*s);
-        yref = 5*sin(0.05*s);
+        xref = 5*cos(0.15*s);
+        yref = 5*sin(0.15*s);
 end
 
 ref = [xref; yref];
diff --git a/tesiema.m b/tesiema.m
index 995a913..da649ef 100644
--- a/tesiema.m
+++ b/tesiema.m
@@ -2,10 +2,14 @@ clc
 clear all
 close all
 
-global x0 ref dref b K saturation tc tfin
+%% global variables
+global x0 ref dref b K SATURATION tc tfin USE_PREDICTION PREDICTION_STEP PREDICTION_SATURATION_TOLERANCE;
 
-TRAJECTORY = 0
+%% variables
+TRAJECTORY = 6
 INITIAL_CONDITIONS = 1
+USE_PREDICTION = false
+PREDICTION_STEPS = 1
 % distance from the center of the unicycle to the point being tracked
 % ATTENZIONE! CI SARA' SEMPRE UN ERRORE COSTANTE DOVUTO A b. Minore b,
 % minore l'errore
@@ -13,24 +17,47 @@ b = 0.2
 % proportional gain
 K = eye(2)*2
 
+tfin=10
 
 % saturation
 % HYP: a diff. drive robot with motors spinning at 100rpm -> 15.7 rad/s.
 % Radius of wheels 10cm. Wheels distanced 15cm from each other
 % applying transformation, v
 % saturation = [1.57, 20];
-saturation = [1.57; 20];
-
+SATURATION = [1.57; 20];
+PREDICTION_SATURATION_TOLERANCE = 0.1;
 
+%% launch simulation
 % initial state
 % In order, [x, y, theta]
 x0 = set_initial_conditions(INITIAL_CONDITIONS)
 % trajectory to track
 [ref, dref] = set_trajectory(TRAJECTORY)
 
-[t, x, ref_t, U] = simulate_discr(60, 0.1);
-plot_results(t, x, ref_t, U);
+global tu uu
 
+%figure(1)
+%USE_PREDICTION = false;
+%[t, x, ref_t, U] = simulate_discr(tfin, 0.05);
+%plot_results(t, x, ref_t, U);
+
+figure(2)
+USE_PREDICTION = true;
+[t1, x1, ref_t1, U1] = simulate_discr(tfin, 0.05);
+plot_results(t1, x1, ref_t1, U1);
+
+figure(3)
+subplot(1, 2, 1)
+plot(tu, uu(1, :))
+subplot(1, 2, 2)
+plot(tu, uu(2, :))
+%plot_results(t, x-x1, ref_t-ref_t1, U-U1);
+
+
+
+%% FUNCTION DECLARATIONS
+
+% Discrete-time simulation
 function [t, x, ref_t, U] = simulate_discr(tfin, tc)
     global ref x0 u_discr
 
@@ -40,7 +67,6 @@ function [t, x, ref_t, U] = simulate_discr(tfin, tc)
     t = 0;
     u_discr = control_act(t, x0);
     U = u_discr';
-
     
     for n = 1:steps
         tspan = [(n-1)*tc n*tc];
@@ -51,7 +77,7 @@ function [t, x, ref_t, U] = simulate_discr(tfin, tc)
         x = [x; z];
         t = [t; v];
         
-        u_discr = control_act(t(end, :), x(end, :));
+        u_discr = control_act(t(end), x(end, :));
         U = [U; ones(length(v), 1)*u_discr'];
     end
 
@@ -59,6 +85,7 @@ function [t, x, ref_t, U] = simulate_discr(tfin, tc)
 end
 
 
+% Continuos-time simulation
 function [t, x, ref, U] = simulate_cont(tfin)
     global ref x0
 
@@ -79,6 +106,7 @@ function [t, x, ref, U] = simulate_cont(tfin)
     ref = double(subs(ref, t'))';
 end
 
+% Plots
 function plot_results(t, x, ref, U)
     subplot(2,2,1)
     hold on