bug: fix u_corr_history not being saved between different calls to u_corr()
+ plot u_track and u_corrmaster
parent
89cb27ff49
commit
1900526b81
|
|
@ -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]);
|
||||
|
||||
u_corr=U_corr_history(:,:, 1);
|
||||
%sim_data.U_corr_history = U_corr_history;
|
||||
u_corr=sim_data.U_corr_history(:,:, 1);
|
||||
|
||||
end
|
||||
|
||||
|
|
|
|||
95
tesiema.m
95
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.SATURATION = [0.5; 0.5];
|
||||
|
||||
sim_data.PREDICTION_HORIZON = data.PREDICTION_HORIZON;
|
||||
sim_data
|
||||
% 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
|
||||
|
||||
[t, q, ref_t, U] = simulate_discr(sim_data);
|
||||
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);
|
||||
|
||||
|
||||
%% 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
|
||||
|
|
|
|||
Loading…
Reference in New Issue