bug: fix u_corr_history not being saved between different calls to u_corr()

+ plot u_track and u_corr

control_act.m

@@ -1,13 +1,14 @@
-function u = control_act(t, q, sim_data)   
+function [u, ut, uc, U_corr_history] = control_act(t, q, sim_data)   
     dc = decouple_matrix(q, sim_data.b);
     ut = utrack(t, q, sim_data);
-    uc = ucorr(t, q, sim_data);
+
+    [uc, U_corr_history] = ucorr(t, q, sim_data);
     u = dc * (ut + uc);
     % saturation
     u = min(sim_data.SATURATION, max(-sim_data.SATURATION, u));
 end
 
-function u_corr = ucorr(t, q, sim_data)
+function [u_corr, U_corr_history] = ucorr(t, q, sim_data)
     pred_hor = sim_data.PREDICTION_HORIZON;
     SATURATION = sim_data.SATURATION;
     PREDICTION_SATURATION_TOLERANCE = sim_data.PREDICTION_SATURATION_TOLERANCE;
@@ -15,14 +16,10 @@ function u_corr = ucorr(t, q, sim_data)
 
     if eq(pred_hor, 0)
         u_corr = zeros(2,1);
+        U_corr_history = zeros(2,1,sim_data.PREDICTION_HORIZON);
         return
     end    
 
-    persistent U_corr_history;
-    if isempty(U_corr_history)
-        U_corr_history = zeros(2, 1, pred_hor);
-    end
-    
     %disp('start of simulation')
     q_prec = q;
     q_pred=zeros(3,1, pred_hor);
@@ -32,7 +29,7 @@ function u_corr = ucorr(t, q, sim_data)
     % predict its future state
 
     % the first step takes in q_k-1 and calculates q_new = q_k
-    % this means that u_track_pred will contain u_track_k-1 and will not
+    % this means that u_track_pred(:,:,1) will contain u_track_k-1 and will not
     % contain u_track_k+C
     for k = 1:pred_hor
         % start from the old (known) state
@@ -40,7 +37,7 @@ function u_corr = ucorr(t, q, sim_data)
         % calculate the inputs, based on the old state
 
         % u_corr is the prediction done at some time in the past, as found in U_corr_history
-        u_corr_ = U_corr_history(:, :, k);
+        u_corr_ = sim_data.U_corr_history(:, :, k);
         % u_track can be calculated from q
         t_ = t + tc*(k-1);
         u_track_ = utrack(t_, q_prec, sim_data);
@@ -135,8 +132,8 @@ function u_corr = ucorr(t, q, sim_data)
     % reshape the vector of vectors to be an array, each element being
     % u_corr_j as a 2x1 vector
     U_corr_history = reshape(U_corr, [2,1,pred_hor]);
-
+    %sim_data.U_corr_history = U_corr_history;
-    u_corr=U_corr_history(:,:, 1);
+    u_corr=sim_data.U_corr_history(:,:, 1);
     
 end
 

tesiema.m

@@ -2,43 +2,61 @@ clc
 clear all
 close all
 
-%% global variables
+TEST = 'sin'
-global K SATURATION PREDICTION_HORIZON PREDICTION_SATURATION_TOLERANCE;
 
-sim_data = load(['tests/sin/common.mat']);
 
+sim_data = load(['tests/' TEST '/common.mat']);
 sim_data.q0 = set_initial_conditions(sim_data.INITIAL_CONDITIONS);
 [ref dref] = set_trajectory(sim_data.TRAJECTORY);
 sim_data.ref = ref;
 sim_data.dref = dref;
 
-spmd (3)
+%sim_data.SATURATION = [0.5; 0.5];
-    worker_index = spmdIndex;
-    data = load(['tests/sin/sin' num2str(spmdIndex) '.mat']);
 
-    sim_data.PREDICTION_HORIZON = data.PREDICTION_HORIZON;
+% spmd (3)
-    sim_data
+%     worker_index = spmdIndex;
+%     data = load(['tests/' TEST '/' num2str(spmdIndex) '.mat']);
+% 
+%     sim_data.PREDICTION_HORIZON = data.PREDICTION_HORIZON;
+%     sim_data.U_corr_history = zeros(2,1,sim_data.PREDICTION_HORIZON);
+%     sim_data
+% 
+%     [t, q, ref_t, U] = simulate_discr(sim_data);
+% 
+%     disp('Done')
+% end
+% 
+% s_ = size(worker_index);
+% for n = 1:s_(2)
+%     figure(n)
+%     plot_results(t{n}, q{n}, ref_t{n}, U{n});
+% end
 
-    [t, q, ref_t, U] = simulate_discr(sim_data);
-end
 
-s_ = size(worker_index);
+data = load(['tests/' TEST '/' num2str(2) '.mat']);
-for n = 1:s_(2)
+sim_data.PREDICTION_HORIZON = data.PREDICTION_HORIZON;
-    figure(n)
+sim_data.U_corr_history = zeros(2,1,sim_data.PREDICTION_HORIZON);
-    plot_results(t{n}, q{n}, ref_t{n}, U{n});
+sim_data
-end
+[t, q, ref_t, U, U_track, U_corr] = simulate_discr(sim_data);
+
+%% 
+plot_results(t,q,ref_t,U, U_track, U_corr);
+
 
 %% FUNCTION DECLARATIONS
 
 % Discrete-time simulation
-function [t, q, ref_t, U] = simulate_discr(sim_data)
+function [t, q, ref_t, U, U_track, U_corr] = simulate_discr(sim_data)
     tc = sim_data.tc;
     steps = sim_data.tfin/tc
     
     q = sim_data.q0';
     t = 0;
-    u_discr = control_act(t, q, sim_data);
+    [u_discr, u_track, u_corr, U_corr_history] = control_act(t, q, sim_data);
+    sim_data.U_corr_history = U_corr_history;
     U = u_discr';
+    U_corr = u_corr';
+    U_track = u_track';
     
     for n = 1:steps
         tspan = [(n-1)*tc n*tc];
@@ -50,8 +68,11 @@ function [t, q, ref_t, U] = simulate_discr(sim_data)
         q = [q; z];
         t = [t; v];
         
-        u_discr = control_act(t(end), q(end, :), sim_data);
+        [u_discr, u_track, u_corr, U_corr_history] = control_act(t(end), q(end, :), sim_data);
+        sim_data.U_corr_history = U_corr_history;
         U = [U; ones(length(v), 1)*u_discr'];
+        U_corr = [U_corr; ones(length(v), 1)*u_corr'];
+        U_track = [U_track; ones(length(v), 1)*u_track'];
     end
 
     ref_t = double(subs(sim_data.ref, t'))';
@@ -78,27 +99,48 @@ function [t, q, ref, U] = simulate_cont(tfin)
     % plot results
     ref = double(subs(ref, t'))';
 end
+%% 
 
 % Plots
-function plot_results(t, x, ref, U)
+function plot_results(t, x, ref, U, U_track, U_corrcd)
-    subplot(2,2,1)
+    subplot(4,2,1)
     hold on
+    title("trajectory / state")
     plot(ref(:, 1), ref(:, 2), "DisplayName", "Ref")
     plot(x(:, 1), x(:, 2), "DisplayName", "state")
     xlabel('x')
     ylabel('y')
     legend()
-    subplot(2,2,3)
+    subplot(4,2,3)
     plot(t, U(:, 1))
     xlabel('t')
     ylabel('input v')
-    subplot(2,2,4)
+    subplot(4,2,4)
     plot(t, U(:, 2))
     xlabel('t')
     ylabel('input w')
 
+    subplot(4,2,5)
+    plot(t, U_corr(:, 1))
+    xlabel('t')
+    ylabel('input correction v')
+    subplot(4,2,6)
+    plot(t, U_corr(:, 2))
+    xlabel('t')
+    ylabel('input correction w')
     
-    subplot(4,4,3)
+    
+    subplot(4,2,7)
+    plot(t, U_track(:, 1))
+    xlabel('t')
+    ylabel('tracking input v')
+    subplot(4,2,8)
+    plot(t, U_track(:, 2))
+    xlabel('t')
+    ylabel('tracking input w')
+    
+    
+    subplot(8,4,3)
     hold on
     xlabel('t')
     ylabel('x')
@@ -107,12 +149,12 @@ function plot_results(t, x, ref, U)
     legend()
     hold off
     
-    subplot(4,4,4)
+    subplot(8,4,4)
     plot(t, ref(:, 1) - x(:, 1));
     xlabel('t')
     ylabel('x error')
     
-    subplot(4,4,7)
+    subplot(8,4,7)
     hold on
     xlabel('t')
     ylabel('y')
@@ -121,8 +163,9 @@ function plot_results(t, x, ref, U)
     legend()
     hold off
     
-    subplot(4,4,8)
+    subplot(8,4,8)
     plot(t, ref(:, 2) - x(:, 2));
     xlabel('t')
     ylabel('y error')
+   
 end