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

control_act: make correction refer to w_r, w_l 

This fixes a substantial bug with 1-step and multistep behaving
differently than track-only

for more information: see notes 10/09/24, but basically what was
  happening was that just v^{corr} was being minimized, for how the
  constraints were built
master
EmaMaker 2024-09-10 16:01:39 +02:00
parent 3ddb3ec4ca
commit f90e3bdb68
2 changed files with 17 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
control_act.m
View File

@ -4,7 +4,7 @@ function [u, ut, uc, U_corr_history, q_pred] = control_act(t, q, sim_data)
[uc, U_corr_history, q_pred] = ucorr(t, q, sim_data);
ut = dc*ut;
uc = dc*uc;
%uc = dc*uc;
%uc = zeros(2,1);
u = ut+uc;
@ -34,7 +34,8 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
f = zeros(2,1);
T_inv = decouple_matrix(q_act, sim_data);
ut = utrack(t, q_act, sim_data);
A = [T_inv; -T_inv];
%A = [T_inv; -T_inv];
A = [eye(2); -eye(2)];
d = T_inv*ut;
b = [s_-d;s_+d];
@ -69,7 +70,7 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
T_inv = decouple_matrix(q_act, sim_data);
% compute inputs (wr, wl)
u_ = T_inv * (u_corr_ + u_track_);
u_ = T_inv * u_track_ + u_corr_;
% map (wr, wl) to (v, w) for unicicle
u_ = diffdrive_to_uni(u_, sim_data);
@ -122,7 +123,9 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
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, [T_inv; -T_inv]);
A_deq = blkdiag(A_deq, [eye(2); -eye(2)]);
d = T_inv*u_track;
b_deq = [b_deq; s_ - d; s_ + d];
@ -134,7 +137,7 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
% squared norm of u_corr. H must be identity,
% PREDICTION_HORIZON*size(u_corr)
H = eye(pred_hor*2);
H = eye(pred_hor*2)*2;
% no linear terms
f = zeros(pred_hor*2, 1);

11
tesi.m
View File

@ -21,7 +21,8 @@ for i = 1:s_(1)
sim_data.(fn{1}) = test_data.(fn{1});
end
%sim_data.r=0.175
%sim_data.r=0.1
%sim_data.b=0.5
sim_data.q0 = set_initial_conditions(sim_data.INITIAL_CONDITIONS);
[ref dref] = set_trajectory(sim_data.TRAJECTORY, sim_data);
sim_data.ref = ref;
@ -95,6 +96,7 @@ function [t, q, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = simula
U_corr = u_corr';
U_track = u_track';
Q_pred(:, :, 1) = q_pred;
y = [];
for n = 1:steps
sim_data.old_u_corr = u_corr;
@ -111,6 +113,7 @@ function [t, q, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = simula
q = [q; z];
t = [t; v];
[u_discr, u_track, u_corr, U_corr_history, q_pred] = control_act(t(end), q(end, :), sim_data);
sim_data.U_corr_history = U_corr_history;
U = [U; ones(length(v), 1)*u_discr'];
@ -118,7 +121,11 @@ function [t, q, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = simula
U_track = [U_track; ones(length(v), 1)*u_track'];
Q_pred(:, :, 1+n) = q_pred;
U_corr_pred_history(:,:,n) = permute(U_corr_history, [3, 1, 2]);
U_corr_pred_history(:,:,n) = permute(U_corr_history, [3, 1, 2]);
y1 = q(:, 1) + sim_data.b * cos(q(:,3));
y2 = q(:, 2) + sim_data.b * sin(q(:,3));
y = [y; y1, y2];
end
ref_t = double(subs(sim_data.ref, t'))';