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

Compare commits

merge into: EmaMaker:master-costfun2
Branches Tags
EmaMaker:diffdrive-costfun
EmaMaker:master-costfun
EmaMaker:master-costfun2
EmaMaker:master
...
pull from: EmaMaker:master
Branches Tags
EmaMaker:diffdrive-costfun
EmaMaker:master-costfun
EmaMaker:master-costfun2
EmaMaker:master

3 Commits
master-cos ... master

Author SHA1 Message Date
EmaMaker 2408bdf472 also plot trajectory and error relative to output position 2024-09-13 20:29:24 +02:00
EmaMaker db2f6f1241 tesi: fix output position vector 2024-09-13 20:29:18 +02:00
EmaMaker ace9febb52 [IMPROVE] build A_deq outside for loop 2024-09-13 20:28:51 +02:00
3 changed files with 15 additions and 21 deletions
Show Stats 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:
% 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]);
A_deq = blkdiag(A_deq, [eye(2); -eye(2)]);
d = T_inv*u_track;
b_deq = [b_deq; s_ - d; s_ + d];
end
A_deq = kron(eye(pred_hor), [eye(2); -eye(2)]);
%A_deq
%b_deq
% unknowns

24
plot_all.m
View File

@ -8,7 +8,6 @@ disp('Photos will start in 3s')
pause(3)
PLOT_TESTS = [
%{
"results-diffdrive/straightline/chill/11-09-2024-16-57-01";
"results-diffdrive/straightline/chill_errortheta_pisixths/11-09-2024-16-57-43";
"results-diffdrive/straightline/chill_errory/11-09-2024-16-59-04";
@ -18,17 +17,16 @@ PLOT_TESTS = [
"results-diffdrive/figure8/chill/11-09-2024-17-00-53";
%"results-diffdrive/figure8/fancyreps/11-09-2024--45-28";
"results-diffdrive/figure8/toofast/11-09-2024-17-01-43";
%}
%"results-unicycle/straightline/chill/11-09-2024-17-07-51";
%"results-unicycle/straightline/chill_errortheta_pisixths/11-09-2024-17-08-35";
%"results-unicycle/straightline/chill_errory/11-09-2024-17-10-00";
%"results-unicycle/straightline/toofast/11-09-2024-17-09-18";
%"results-unicycle/circle/start_center/11-09-2024-17-10-48";
"results-unicycle/straightline/chill/11-09-2024-17-07-51";
"results-unicycle/straightline/chill_errortheta_pisixths/11-09-2024-17-08-35";
"results-unicycle/straightline/chill_errory/11-09-2024-17-10-00";
"results-unicycle/straightline/toofast/11-09-2024-17-09-18";
"results-unicycle/circle/start_center/11-09-2024-17-10-48";
"results-unicycle/square/11-09-2024-17-17-21";
%"results-unicycle/figure8/chill/11-09-2024-17-11-53";
"results-unicycle/figure8/chill/11-09-2024-17-11-53";
%"results-unicycle/figure8/fancyreps/11-09-2024--45-28";
%"results-unicycle/figure8/toofast/11-09-2024-17-12-45";
"results-unicycle/figure8/toofast/11-09-2024-17-12-45";
]
s_ = size(PLOT_TESTS)
@ -60,7 +58,7 @@ for i = 1:s_(1)
pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_triple_input_1x2.png'])
pause(1); clf; plot_tripleinput(t{n}, sim_data{n}.SATURATION, U{n}, U_track{n}, U_corr{n}, 1, in1, in2, m1, m2)
pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_triple_input_2x1.png'])
%pause(1); clf; plot_input(t{n}, sim_data{n}.SATURATION, U_track{n}, 'track')
%pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_track_input.png'])
%pause(1); clf; plot_input(t{n}, sim_data{n}.SATURATION, U_corr{n}, 'corr')
@ -70,6 +68,11 @@ for i = 1:s_(1)
pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_total_input_1x2.png'])
pause(1); clf; plot_input(t{n},sim_data{n}.SATURATION, U{n}, 1, '', in1, in2, m1, m2)
pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_total_input_2x1.png'])
pause(1); clf; plot_trajectory(t{n}, ref_t{n}, y{n})
pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_trajectory_out.png'])
pause(1); clf; plot_error(t{n}, ref_t{n}, y{n})
pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_error_out.png'])
end
% correction difference (multistep, 1-step)
@ -95,5 +98,4 @@ for i = 1:s_(1)
pause(0.5); export_fig(gcf, '-transparent', [dir, 'input_diff_1step_multistep_1x2.png'])
pause(1); clf; plot_input_diff(t{2}, t{3}, U{2}, U{3}, 1, ['\textbf{$$' in1 '^{1step}$$}'], ['\textbf{$$' in1 '^{multistep}$$}'], ['\textbf{$$' in2 '^{1step}$$}'], ['\textbf{$$' in2 '^{multistep}$$}'], m1, m2)
pause(0.5); export_fig(gcf, '-transparent', [dir, 'input_diff_1step_multistep_2x1.png'])
end

2
tesi.m
View File

@ -149,7 +149,7 @@ function [t, q, y, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = sim
y1 = q(:, 1) + sim_data.b * cos(q(:,3));
y2 = q(:, 2) + sim_data.b * sin(q(:,3));
y = [y; y1, y2];
y = [y; [y1, y2]];
end
ref_t = double(subs(sim_data.ref, t'))';