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

minimize vcorr^2 on correction before decoupling

master-costfun
EmaMaker 2024-09-16 18:27:41 +02:00
parent 41f0d66851
commit cceb2f1379
2 changed files with 6 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
control_act.m
View File

@ -32,8 +32,9 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
T_inv = decouple_matrix(q_act, sim_data); T_inv = decouple_matrix(q_act, sim_data);
ut = utrack(t, q_act, sim_data); ut = utrack(t, q_act, sim_data);
H = 2 * (T_inv') * T_inv; % minimize v
%H = eye(2); H = 2*eye(2);
f = zeros(2,1); f = zeros(2,1);
A = [T_inv; -T_inv]; A = [T_inv; -T_inv];
%A = [eye(2); -eye(2)]; %A = [eye(2); -eye(2)];
@ -121,20 +122,15 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
% 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 = []; A_deq = [];
b_deq = []; b_deq = [];
H1 = [];
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);
d = T_inv*u_track; d = T_inv*u_track;
H1 = blkdiag(H1, T_inv);
H2 = blkdiag(H2, T_inv');
A_deq = blkdiag(A_deq, [T_inv; -T_inv]); A_deq = blkdiag(A_deq, [T_inv; -T_inv]);
b_deq = [b_deq; s_ - d; s_ + d]; b_deq = [b_deq; s_ - d; s_ + d];
end end
H = H1'*H1;
%A_deq = kron(eye(pred_hor), [eye(2); -eye(2)]); %A_deq = kron(eye(pred_hor), [eye(2); -eye(2)]);
%A_deq %A_deq
%b_deq %b_deq
@ -143,7 +139,8 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
% squared norm of u_corr. H must be identity, % squared norm of u_corr. H must be identity,
% PREDICTION_HORIZON*size(u_corr) % PREDICTION_HORIZON*size(u_corr)
%H = eye(pred_hor*2)*2; %H = eye(pred_hor*2)*2;
%H = kron(eye(pred_hor), [1,0;0,0]); H = kron(eye(pred_hor), 2*eye(2));
%H = kron(eye(pred_hor), 2*ones(2,2));
% no linear terms % no linear terms
f = zeros(pred_hor*2, 1); f = zeros(pred_hor*2, 1);

2
tesi.m
View File

@ -8,6 +8,7 @@ ROBOT = 'diffdrive'
TESTS = ["circle/start_center"] TESTS = ["circle/start_center"]
% main % main
s_ = size(TESTS); s_ = size(TESTS);
for i = 1:length(TESTS) for i = 1:length(TESTS)
clearvars -except i s_ TESTS ROBOT clearvars -except i s_ TESTS ROBOT
@ -24,7 +25,6 @@ for i = 1:length(TESTS)
sim_data.(fn{1}) = test_data.(fn{1}); sim_data.(fn{1}) = test_data.(fn{1});
end end
sim_data.r = 0.06
% set trajectory and starting conditions % set trajectory and starting conditions
sim_data.q0 = set_initial_conditions(sim_data.INITIAL_CONDITIONS); sim_data.q0 = set_initial_conditions(sim_data.INITIAL_CONDITIONS);
[ref dref] = set_trajectory(sim_data.TRAJECTORY, sim_data); [ref dref] = set_trajectory(sim_data.TRAJECTORY, sim_data);