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

control_act: fix multi-step mpc

EmaMaker 2024-07-29 15:22:36 +02:00
parent 9e4c0e506f
commit 8234f7bdb3
1 changed files with 35 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
control_act.m
View File

@ -14,14 +14,20 @@ function [u_corr, U_corr_history] = ucorr(t, q, sim_data)
PREDICTION_SATURATION_TOLERANCE = sim_data.PREDICTION_SATURATION_TOLERANCE;
tc = sim_data.tc;
u_corr = zeros(2,1);
U_corr_history = zeros(2,1,sim_data.PREDICTION_HORIZON);
if eq(pred_hor, 0)
u_corr = zeros(2,1);
U_corr_history = zeros(2,1,sim_data.PREDICTION_HORIZON);
return
end
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')
q_prec = q;
q_act = q;
q_pred=zeros(3,1, pred_hor);
u_track_pred=zeros(2,1, pred_hor);
T_inv_pred=zeros(2,2, pred_hor);
@ -34,43 +40,43 @@ function [u_corr, U_corr_history] = ucorr(t, q, sim_data)
for k = 1:pred_hor
% start from the old (known) state
% calculate the inputs, based on the old 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_ = 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);
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_prec, sim_data.b);
T_inv = decouple_matrix(q_act, sim_data.b);
u_ = T_inv * (u_corr_ + u_track_);
% calc the state integrating with euler
x_new = q_prec(1) + tc*u_(1) * cos(q_prec(3));
y_new = q_prec(2) + tc*u_(1) * sin(q_prec(3));
theta_new = q_prec(3) + tc*u_(2);
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(:,:,k) = q_new;
%q_pred(:,:,k) = q_new;
u_track_pred(:,:,k) = u_track_;
T_inv_pred(:,:,k) = T_inv;
% Prepare old state for next iteration
q_prec = q_new;
q_act = q_new;
end
%disp('end of simulation')
%q_prec
% calculate u_track_k+C
u_track_pred(:,:,pred_hor+1) = utrack(t+tc*pred_hor, q_prec, sim_data);
% remove u_track_k-1
%u_track_pred = u_track_pred(:,:,2:end);
T_inv_pred(:,:,pred_hor+1) = decouple_matrix(q_prec, sim_data.b);
%T_inv_pred = T_inv_pred(:,:,2:end);
% calculate u_track_k+C and T_inv_k+C, needed for constraints
% u_track_pred(:,:,pred_hor) = utrack(t+tc*pred_hor, q_act, sim_data);
% T_inv_pred(:,:,pred_hor) = decouple_matrix(q_act, sim_data.b);
%disp('end of patching data up')
@ -98,6 +104,7 @@ function [u_corr, U_corr_history] = ucorr(t, q, sim_data)
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);
@ -111,14 +118,13 @@ function [u_corr, U_corr_history] = ucorr(t, q, sim_data)
A_deq = [A_deq, column];
d = T_inv*u_track;
b_deq = [b_deq; SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE - d;
SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE + d];
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)*2;
@ -131,9 +137,11 @@ function [u_corr, U_corr_history] = ucorr(t, q, sim_data)
% 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]);
%sim_data.U_corr_history = U_corr_history;
u_corr=sim_data.U_corr_history(:,:, 1);
% first result is what to do now
u_corr=U_corr_history(:,:, 1);
end