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

[IMPROVE] build A_deq outside for loop

master-costfun2
EmaMaker 2024-09-11 22:20:31 +02:00
parent a9bf86cda8
commit 854247dee1
1 changed files with 1 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
control_act.m
View File

@ -118,23 +118,15 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
% A will be at most PREDICTION_HORIZON * 2 * 2 (2: size of T_inv; 2: % 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 % 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) % vectors in u_corr times the number of elements [2] in each vector)
A_deq = [];
b_deq = []; b_deq = [];
s_ = SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE;
for k=1:pred_hor for k=1:pred_hor
T_inv = T_inv_pred(:,:,k); T_inv = T_inv_pred(:,:,k);
u_track = u_track_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]);
A_deq = blkdiag(A_deq, [eye(2); -eye(2)]);
d = T_inv*u_track; d = T_inv*u_track;
b_deq = [b_deq; s_ - d; s_ + d]; b_deq = [b_deq; s_ - d; s_ + d];
end end
A_deq = kron(eye(pred_hor), [eye(2); -eye(2)]);
%A_deq %A_deq
%b_deq %b_deq
% unknowns % unknowns