diff --git a/control_act.m b/control_act.m index 7e72174..fa68c58 100644 --- a/control_act.m +++ b/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 + 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); @@ -134,9 +131,9 @@ 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]); - - u_corr=U_corr_history(:,:, 1); + U_corr_history = reshape(U_corr, [2,1,pred_hor]); + %sim_data.U_corr_history = U_corr_history; + u_corr=sim_data.U_corr_history(:,:, 1); end diff --git a/tesiema.m b/tesiema.m index d4f6b73..353dce5 100644 --- a/tesiema.m +++ b/tesiema.m @@ -2,43 +2,61 @@ clc clear all close all -%% global variables -global K SATURATION PREDICTION_HORIZON PREDICTION_SATURATION_TOLERANCE; +TEST = 'sin' -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) - worker_index = spmdIndex; - data = load(['tests/sin/sin' num2str(spmdIndex) '.mat']); - - sim_data.PREDICTION_HORIZON = data.PREDICTION_HORIZON; - sim_data +%sim_data.SATURATION = [0.5; 0.5]; - [t, q, ref_t, U] = simulate_discr(sim_data); -end +% spmd (3) +% 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 + + +data = load(['tests/' TEST '/' num2str(2) '.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, U_track, U_corr] = simulate_discr(sim_data); + +%% +plot_results(t,q,ref_t,U, U_track, U_corr); -s_ = size(worker_index); -for n = 1:s_(2) - figure(n) - plot_results(t{n}, q{n}, ref_t{n}, U{n}); -end %% 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) - subplot(2,2,1) +function plot_results(t, x, ref, U, U_track, U_corrcd) + 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