EmaMaker
/
thesis
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

control_act: 1-step into its own if-statement

master
EmaMaker 2024-09-10 13:04:57 +02:00
parent f5972fd508
commit 3ddb3ec4ca
3 changed files with 124 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

220
control_act.m
View File

@ -5,6 +5,7 @@ function [u, ut, uc, U_corr_history, q_pred] = control_act(t, q, sim_data)
ut = dc*ut;
uc = dc*uc;
%uc = zeros(2,1);
u = ut+uc;
% saturation
@ -25,113 +26,130 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
q_pred = [];
s_ = SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE;
if eq(pred_hor, 0)
return
end
if pred_hor > 1
% move the horizon over 1 step and add trailing zeroes
U_corr_history = cat(3, sim_data.U_corr_history(:,:, 2:end), zeros(2,1));
end
%disp('start of simulation')
% for each step in the prediction horizon, integrate the system to
% predict its future state
for k = 1:pred_hor
% start from the old (known) state
% compute 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_track can be computed from q
t_ = t + tc * (k-1);
u_track_ = utrack(t_, q_act, sim_data);
elseif eq(pred_hor, 1)
H = eye(2);
f = zeros(2,1);
T_inv = decouple_matrix(q_act, sim_data);
% compute inputs (wr, wl)
u_ = T_inv * (u_corr_ + u_track_);
% map (wr, wl) to (v, w) for unicicle
u_ = diffdrive_to_uni(u_, sim_data);
ut = utrack(t, q_act, sim_data);
A = [T_inv; -T_inv];
% integrate unicycle
theta_new = q_act(3) + tc*u_(2);
% compute the state integrating with euler
%x_new = q_act(1) + tc*u_(1) * cos(q_act(3));
%y_new = q_act(2) + tc*u_(1) * sin(q_act(3));
% compute the state integrating via runge-kutta
x_new = q_act(1) + tc*u_(1) * cos(q_act(3) + 0.5*tc*u_(2));
y_new = q_act(2) + tc*u_(1) * sin(q_act(3) + 0.5*tc*u_(2));
q_new = [x_new; y_new; theta_new];
% save history
q_pred = [q_pred; q_new'];
u_track_pred(:,:,k) = u_track_;
T_inv_pred(:,:,k) = T_inv;
% Prepare old state for next iteration
q_act = q_new;
end
%{
Now setup the qp problem
It needs:
- Unknowns, u_corr at each timestep. Will be encoded as a vector of
vectors, in which every two elements is a u_j
i.e. (u_1; u_2; u_3; ...; u_C) = (v_1; w_1; v_2, w_2; v_3, w_3; ...
; v_C, w_C)
It is essential that the vector stays a column, so that u'u is the
sum of the squared norms of each u_j
- Box constraints: a constraint for each timestep in the horizon.
Calculated using the predicted state and inputs. They need to be
put in matrix (Ax <= b) form
%}
% box constrains
% A becomes sort of block-diagonal
% A will be at most PREDICTION_HORIZON * 2 * 2 (2: size of T_inv; 2:
% accounting for T_inv and -T_inv) by PREDICTION_HORIZON (number of
% vectors in u_corr times the number of elements [2] in each vector)
A_deq = [];
b_deq = [];
s_ = SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE;
for k=1:pred_hor
T_inv = T_inv_pred(:,:,k);
u_track = u_track_pred(:,:,k);
% [T_inv; -T_inv] is a 4x2 matrix
A_deq = blkdiag(A_deq, [T_inv; -T_inv]);
d = T_inv*u_track;
b_deq = [b_deq; s_ - d; s_ + d];
end
d = T_inv*ut;
b = [s_-d;s_+d];
% solve qp problem
options = optimoptions('quadprog', 'Display', 'off');
u_corr = quadprog(H, f, A, b, [],[],[],[],[],options);
q_pred = q_act;
U_corr_history(:,:,1) = u_corr;
return
else
%if pred_hor > 1
% move the horizon over 1 step and add trailing zeroes
U_corr_history = cat(3, sim_data.U_corr_history(:,:, 2:end), zeros(2,1));
%end
%A_deq
%b_deq
% unknowns
% squared norm of u_corr. H must be identity,
% PREDICTION_HORIZON*size(u_corr)
H = eye(pred_hor*2);
% no linear terms
f = zeros(pred_hor*2, 1);
% solve qp problem
options = optimoptions('quadprog', 'Display', 'off');
U_corr = quadprog(H, f, A_deq, b_deq, [],[],[],[],[],options);
%U_corr = lsqnonlin(@(pred_hor) ones(pred_hor, 1), U_corr_history(:,:,1), [], [], A_deq, b_deq, [], []);
% reshape the vector of vectors to be an array, each element being
% u_corr_j as a 2x1 vector
% and add the prediction at t_k+C
U_corr_history = reshape(U_corr, [2,1,pred_hor]);
% first result is what to do now
u_corr=U_corr_history(:,:, 1);
%disp('start of simulation')
% for each step in the prediction horizon, integrate the system to
% predict its future state
for k = 1:pred_hor
% start from the old (known) state
% compute 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_track can be computed from q
t_ = t + tc * (k-1);
u_track_ = utrack(t_, q_act, sim_data);
T_inv = decouple_matrix(q_act, sim_data);
% compute inputs (wr, wl)
u_ = T_inv * (u_corr_ + u_track_);
% map (wr, wl) to (v, w) for unicicle
u_ = diffdrive_to_uni(u_, sim_data);
% integrate unicycle
theta_new = q_act(3) + tc*u_(2);
% compute the state integrating with euler
%x_new = q_act(1) + tc*u_(1) * cos(q_act(3));
%y_new = q_act(2) + tc*u_(1) * sin(q_act(3));
% compute the state integrating via runge-kutta
x_new = q_act(1) + tc*u_(1) * cos(q_act(3) + 0.5*tc*u_(2));
y_new = q_act(2) + tc*u_(1) * sin(q_act(3) + 0.5*tc*u_(2));
q_new = [x_new; y_new; theta_new];
% save history
q_pred = [q_pred; q_new'];
u_track_pred(:,:,k) = u_track_;
T_inv_pred(:,:,k) = T_inv;
% Prepare old state for next iteration
q_act = q_new;
end
%{
Now setup the qp problem
It needs:
- Unknowns, u_corr at each timestep. Will be encoded as a vector of
vectors, in which every two elements is a u_j
i.e. (u_1; u_2; u_3; ...; u_C) = (v_1; w_1; v_2, w_2; v_3, w_3; ...
; v_C, w_C)
It is essential that the vector stays a column, so that u'u is the
sum of the squared norms of each u_j
- Box constraints: a constraint for each timestep in the horizon.
Calculated using the predicted state and inputs. They need to be
put in matrix (Ax <= b) form
%}
% box constrains
% A becomes sort of block-diagonal
% A will be at most PREDICTION_HORIZON * 2 * 2 (2: size of T_inv; 2:
% accounting for T_inv and -T_inv) by PREDICTION_HORIZON (number of
% vectors in u_corr times the number of elements [2] in each vector)
A_deq = [];
b_deq = [];
s_ = SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE;
for k=1:pred_hor
T_inv = T_inv_pred(:,:,k);
u_track = u_track_pred(:,:,k);
% [T_inv; -T_inv] is a 4x2 matrix
A_deq = blkdiag(A_deq, [T_inv; -T_inv]);
d = T_inv*u_track;
b_deq = [b_deq; s_ - d; s_ + d];
end
%A_deq
%b_deq
% unknowns
% squared norm of u_corr. H must be identity,
% PREDICTION_HORIZON*size(u_corr)
H = eye(pred_hor*2);
% no linear terms
f = zeros(pred_hor*2, 1);
% solve qp problem
options = optimoptions('quadprog', 'Display', 'off');
U_corr = quadprog(H, f, A_deq, b_deq, [],[],[],[],[],options);
%U_corr = lsqnonlin(@(pred_hor) ones(pred_hor, 1), U_corr_history(:,:,1), [], [], A_deq, b_deq, [], []);
% reshape the vector of vectors to be an array, each element being
% u_corr_j as a 2x1 vector
% and add the prediction at t_k+C
U_corr_history = reshape(U_corr, [2,1,pred_hor]);
% first result is what to do now
u_corr=U_corr_history(:,:, 1);
end
end
function u_track = utrack(t, q, sim_data)

BIN
fig.fig Normal file
View File

Binary file not shown.

8
tesi.m
View File

@ -3,7 +3,7 @@ clear all
close all
%TESTS = ["sin_faster", "sin", "circle", "straightline", "reverse_straightline"]
TESTS = ["figure8/fancyreps"]
TESTS = ["circle/start_center"]
s_ = size(TESTS);
@ -20,7 +20,8 @@ for i = 1:s_(1)
for fn = fieldnames(test_data)'
sim_data.(fn{1}) = test_data.(fn{1});
end
%sim_data.r=0.175
sim_data.q0 = set_initial_conditions(sim_data.INITIAL_CONDITIONS);
[ref dref] = set_trajectory(sim_data.TRAJECTORY, sim_data);
sim_data.ref = ref;
@ -104,7 +105,8 @@ function [t, q, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = simula
z0 = q(end, :);
%[v, z] = ode45(@sistema_discr, tspan, z0, u_discr);
[v, z] = ode45(@(v, z) sistema_discr(v, z, u_discr, sim_data), tspan, z0);
opt = odeset('MaxStep', 0.005);
[v, z] = ode45(@(v, z) sistema_discr(v, z, u_discr, sim_data), tspan, z0, opt);
q = [q; z];
t = [t; v];