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

[UNSUCCESS] try to minimize vcorr^2

master-costfun2
EmaMaker 2024-09-16 18:27:41 +02:00
parent 41f0d66851
commit eee95b0059
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
control_act.m
View File

@ -32,7 +32,8 @@ 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; TH = T_inv;
H = 2 * (TH') * [1, 0; 0, 0] * [1, 0; 0, 0] * TH;
%H = eye(2); %H = eye(2);
f = zeros(2,1); f = zeros(2,1);
A = [T_inv; -T_inv]; A = [T_inv; -T_inv];
@ -127,8 +128,9 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
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); TH = [1, 0; 0, 0] * T_inv;
H2 = blkdiag(H2, T_inv'); H1 = blkdiag(H1, TH);
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