%% task 1.2
load('Messdaten.mat');

xw = x2.Data(:,1);
phi = x2.Data(:,2);
vw = x2.Data(:,3);
w = x2.Data(:,4);
t = x2.Time;

wdot = diff(w) ./ diff(t);
phi = phi(1:length(phi)-1);
w = w(1:length(w)-1);
t = t(1:length(t)-1);
vw = vw(1:length(vw)-1);
u2 = u2.data(1:length(u2.data)-1);

xhat = sdpvar(4,1);
A1 = g*sin(phi) + wdot.*(((cos(phi)).^2)-l)-l*w.*w.*sin(phi).*cos(phi);
A2 = -l*wdot + g*sin(phi);
A3 = -u2;
A4 = vw;
A = [A1 A2 A3 A4];
e = A*xhat;

options = sdpsettings('verbose', 0);
diagnostics_L2 = optimize([], e'*e, options);
if diagnostics_L2.problem > 0
    error('Fehler bei L2-Optimierung')
end
xhat = value(xhat);
disp(xhat);