%% Educated guess
t = id1.t; y = id1.y; u = id1.u;
figure; % Create a figure
subplot(2,1,1); % Create a subplot on the figure for output
plot(t,y); % Plot the data on the new subplot
subplot(2,1,2); % Create a second subplot for the input signal
plot(t,u);

%% Observations: first output sample nonzero y(0) != 0
% Therefore we remove this offset
y = y - y(1)

%% Identification
x0 = [1 1 1] % Poles only TF: G(s) = 1 / (a*s^2 + b*s + c)

% Objective function: minimize sum of squares of residuals
J = @(x) ( ...
    sum( ...
    ( ...
    y - lsim(tf(1,x), u, t) ...
    ).^2 ...
    ))

[x, fval]  = optimize(J, x0, [], [], [], [], ...
                      [], [], [], [], ...
                      optimset('display', 'iter'))
                  
%% Recounstruct the transfer function
% representing the plant
G = tf([1], x)

% Check the model out
vplot(G, id1);
vplot(G, id1.valid);

%% Identification v2
x0 = [1 1 1 1 1] % Poles and zeros TF: G(s) = (q*s + r) / (a*s^2 + b*s + c)
%     --- ***** : B(s)/A(s)
%      B    A

% Objective function: minimize sum of squares of residuals
J = @(x) ( ...
    sum( ...
    ( ...
    y - lsim(tf(x(1:2), x(3:5)), u, t) ...
    ).^2 ...
    ))

[x, fval] = optimize(J, x0, [], [], [], [], ...
                      [], [], [], [], ...
                      optimset('display', 'iter'))

%% Recounstruct the transfer function
% representing the plant
G = tf(x(1:2), x(3:5))

% Check the model out
vplot(G, id1);
vplot(G, id1.valid);



