X0=[34.29; 0; 0.1490; 0.3341; 28.220; 0.2405];
y=matched_ests(:,2);
y_dot=matched_ests(:,3);
y_ddot=matched_ests(:,4);
y_tdot=matched_ests(:,5);
u=matched_ests(:,6);
opt_f = @(X)opt_func_sync_gen(X, y, y_dot, y_ddot, y_tdot, u);
options = optimoptions('fmincon', 'MaxIterations', 10000,...
'MaxFunEvals', 10000, 'UseParallel', true);
A=[];
b=[];
Aeq=[];
beq=[];
lb = zeros(6,1);
ub=[100;100;100;100;100;100];
nonlcon = [];
[Y,fval] = fmincon(opt_f,X0,A,b,Aeq,beq,lb,ub,nonlcon,options);
b1 = Y(1);
b2 = Y(2);
b3 = Y(3);
b4 = Y(4);
P = Y(5);
E = Y(6);
fprintf('%.3f %.3f %.3f %.3f %.3f %.3f %.3f',b1,b2,b3,b4,P,E);
function [obj_func_value] = opt_func_sync_gen(X, y, y_dot, y_ddot, y_tdot, u)
b1 = X(1);
b2 = X(2);
b3 = X(3);
b4 = X(4);
P = X(5);
E = X(6);
sin_y = sin(y);
cos_y = cos(y);
vector_func = (E*b1*(sin_y).^2 - b4*P.*(sin_y) + b2*b4.*y_dot.*sin_y...
+ b4*y_ddot.*sin_y + b1*b3.*(sin_y.^2).*cos_y...
+ b2*(sin_y.*y_ddot - (y_dot.^2).*cos_y)...
+ P.*y_dot.*cos_y - b1.*sin_y.^2.*u ...
+ y_tdot.*sin_y - y_dot.*y_ddot.*cos_y).^2;
obj_func_value = sum(vector_func);
end
