rename state x to q

.gitignore

@@ -0,0 +1 @@
+*.asv

control_act.m

@@ -1,26 +1,69 @@
-function u = control_act(t, x)
-    global ref dref K b saturation
+function u = control_act(t, q)
+    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;
+    err = ref_s - feedback(q);
+    u_track = dref_s + K*err;
     
-    theta = x(3);
+    theta = q(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 u_corr 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)
+function q_track = feedback(q)
     global b
-    x_track = [ x(1) + b*cos(x(3)); x(2) + b*sin(x(3))  ];
+    q_track = [ q(1) + b*cos(q(3)); q(2) + b*sin(q(3))  ];
 end
 
 

set_trajectory.m

@@ -11,14 +11,25 @@ switch i
         xref = 10*s;
         yref = 0;
     case 2
-        xref = 5*cos(s);
-        yref = 5*sin(s);
+        % straight line, with initial error
+        xref = 5 + 0.5*s;
+        yref = 0;
     case 3
+        % straight line, initial error, faster
+        xref = 5 + 10*s;
+        yref = 0;
+    case 4
+        xref = 2.5*cos(s);
+        yref = 2.5*sin(s);
+    case 5
         xref = 15*cos(s);
         yref = 15*sin(s);
-    case 4
-        xref = 5*cos(0.05*s)
-        yref = 5*cos(0.05*s)
+    case 6
+        xref = 0.4*s; 
+        yref = cos(0.4*s);
+    case 7
+        xref = 5*cos(0.05*s);
+        yref = 5*sin(0.05*s);
 end
 
 ref = [xref; yref];

sistema.m

@@ -1,11 +1,3 @@
-function x = sistema(t, x)
-    x = unicycle(t, x, control_act(t, x));
-end
-
-function dx = unicycle(t, x, u)
-    % u is (v;w)
-    % x is (x; y; theta)
-    theta = x(3);
-    G_x = [cos(theta), 0; sin(theta), 0; 0, 1];
-    dx = G_x*u;
+function q = sistema(t, q)
+    q = unicycle(t, q, control_act(t, q));
 end

sistema_discr.m

@@ -0,0 +1,4 @@
+function x = sistema_discr(t, q)
+    global u_discr
+    q = unicycle(t, q, u_discr);
+end

tesiema.m

@@ -2,78 +2,154 @@ clc
 clear all
 close all
 
-global x0 ref dref b K saturation
+%% global variables
+global q0 ref dref b K SATURATION tc tfin USE_PREDICTION PREDICTION_STEP PREDICTION_SATURATION_TOLERANCE;
 
-TRAJECTORY = 4
-INITIAL_CONDITIONS = 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
 b = 0.2
 % proportional gain
-K = eye(2)*2.5
+K = eye(2)*2
+
+tfin=30
+
 % saturation
-saturation = [5; 0.5];
-
+% 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; 1];
+PREDICTION_SATURATION_TOLERANCE = 0.0;
 
+%% launch simulation
 % initial state
 % In order, [x, y, theta]
-x0 = set_initial_conditions(INITIAL_CONDITIONS)
+q0 = set_initial_conditions(INITIAL_CONDITIONS)
 % trajectory to track
 [ref, dref] = set_trajectory(TRAJECTORY)
 
+global tu uu
 
-% simulation time
-tspan = 0:0.1:60;
-% execute simulation
-[t, x] = ode45(@sistema, tspan, x0);
+figure(1)
+USE_PREDICTION = false;
+[t, q, ref_t, U] = simulate_discr(tfin, 0.1);
+plot_results(t, q, ref_t, U);
 
-% recalc and save input at each timestep
-ts = size(t);
-rows = ts(1);
-U = zeros(rows, 2);
-for row = 1:rows
-    U(row, :) = control_act(t(row), x(row, :));
+figure(2)
+USE_PREDICTION = true;
+[t1, q1, ref_t1, U1] = simulate_discr(tfin, 0.1);
+plot_results(t1, q1, 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, q, ref_t, U] = simulate_discr(tfin, tc)
+    global ref q0 u_discr
+
+    steps = tfin/tc
+    
+    q = q0';
+    t = 0;
+    u_discr = control_act(t, q0);
+    U = u_discr';
+    
+    for n = 1:steps
+        tspan = [(n-1)*tc n*tc];
+        z0 = q(end, :);
+        
+        [v, z] = ode45(@sistema, tspan, z0);
+
+        q = [q; z];
+        t = [t; v];
+        
+        u_discr = control_act(t(end), q(end, :));
+        U = [U; ones(length(v), 1)*u_discr'];
+    end
+
+    ref_t = double(subs(ref, t'))';
 end
 
-% plot results
-ref_t = double(subs(ref, t'))';
 
-subplot(3,2,1)
-hold on
-xlabel('t')
-ylabel('x')
-plot(t, ref_t(:, 1));
-plot(t, x(:, 1));
-legend()
-hold off
+% Continuos-time simulation
+function [t, q, ref, U] = simulate_cont(tfin)
+    global ref q0
 
-subplot(3,2,2)
-plot(t, ref_t(:, 1) - x(:, 1));
-xlabel('t')
-ylabel('x error')
+    % simulation time
+    tspan = linspace(0, tfin);
+    % execute simulation
+    [t, q] = ode45(@sistema, tspan, q0);
+    
+    % recalc and save input at each timestep
+    ts = size(t);
+    rows = ts(1);
+    U = zeros(rows, 2);
+    for row = 1:rows
+        U(row, :) = control_act(t(row), q(row, :));
+    end
+    
+    % plot results
+    ref = double(subs(ref, t'))';
+end
 
-subplot(3,2,3)
-hold on
-xlabel('t')
-ylabel('y')
-plot(t, ref_t(:, 2));
-plot(t, x(:, 2));
-legend()
-hold off
-
-subplot(3,2,4)
-plot(t, ref_t(:, 2) - x(:, 2));
-xlabel('t')
-ylabel('y error')
-
-
-subplot(3,2,5)
-plot(t, U(:, 1))
-xlabel('t')
-ylabel('input v')
-
-subplot(3,2,6)
-plot(t, U(:, 2))
-xlabel('t')
-ylabel('input w')
+% Plots
+function plot_results(t, x, ref, U)
+    subplot(2,2,1)
+    hold on
+    plot(ref(:, 1), ref(:, 2), "DisplayName", "Ref")
+    plot(x(:, 1), x(:, 2), "DisplayName", "state")
+    xlabel('x')
+    ylabel('y')
+    legend()
+    subplot(2,2,3)
+    plot(t, U(:, 1))
+    xlabel('t')
+    ylabel('input v')
+    subplot(2,2,4)
+    plot(t, U(:, 2))
+    xlabel('t')
+    ylabel('input w')
+    
+    
+    subplot(4,4,3)
+    hold on
+    xlabel('t')
+    ylabel('x')
+    plot(t, ref(:, 1), "DisplayName", "X_{ref}");
+    plot(t, x(:, 1), "DisplayName", "X");
+    legend()
+    hold off
+    
+    subplot(4,4,4)
+    plot(t, ref(:, 1) - x(:, 1));
+    xlabel('t')
+    ylabel('x error')
+    
+    subplot(4,4,7)
+    hold on
+    xlabel('t')
+    ylabel('y')
+    plot(t, ref(:, 2), "DisplayName", "Y_{ref}");
+    plot(t, x(:, 2), "DisplayName", "Y");
+    legend()
+    hold off
+    
+    subplot(4,4,8)
+    plot(t, ref(:, 2) - x(:, 2));
+    xlabel('t')
+    ylabel('y error')
+end

unicycle.m

@@ -0,0 +1,7 @@
+function dx = unicycle(t, x, u)
+    % u is (v;w)
+    % x is (x; y; theta)
+    theta = x(3);
+    G_x = [cos(theta), 0; sin(theta), 0; 0, 1];
+    dx = G_x*u;
+end