clear
delay = 0.1;
zeta = 1/sqrt(2);
omega = 5;
num = omega^2;
den = [1, 2*zeta*omega, omega^2];
G = tf(num, den, 'InputDelay', delay);

Ts = 0.2;
G0 = ss(tf(num, den));
M1 = expm([G0.A, G0.B; zeros(size(G0.B, 2), size([G0.A, G0.B], 2))]*0.2);
A = M1(1:2, 1:2);
b = M1(1:2, 3);
M2 = expm([G0.A, G0.B; zeros(size(G0.B, 2), size([G0.A, G0.B], 2))]*0.1);
b0 = M2(1:2, 3);

G1 = ss(A, b, G0.C, G0.D, 0.2);
G2 = ss(A, b0, G0.C, G0.D, 0.2);

[num1, den1] = tfdata(G1, 'v');
num2 = tfdata(G2, 'v');

Ts = 0.2;
N = 3000;
t = (0:N-1)*Ts;
u = randn(N, 1);
start = 30;

theta_true = [den1(2:end), num1(2:end), num2(2:end)];

trial = 100;
Theta = zeros(trial, 6);

for i = 1:trial
    
    Ts = 0.2;
    N = 3000;
    t = (0:N-1)*Ts;
    u = randn(N, 1);
    w = randn(N, 1)*sqrt(0.1);
    
    y = lsim(G, u, t, 'zoh')+w;
    
    gamma = 0.0001;
    
    fun = @(theta) eval_func(theta, t, u, y, start, gamma, Ts);
    theta_hat = fminsearch(fun, [-0.7 0.2 0.3 0.2 0.1 0.2]);
    fprintf('%d,\t', i);
    fprintf('%e,', theta_hat);
    fprintf('\n');
    
    Theta(i, :) = theta_hat;
end

disp(theta_true)
disp(mean(Theta));
disp(std(Theta));


function out = eval_func(theta, t, u, y, start, gamma, Ts)

a = theta(1:2);
b = theta(3:4);
bt = theta(5:6);

G = tf(b(:)', [1, a(:)'], Ts);
Gt = tf(bt(:)', [1, a(:)'], Ts);

y1 = lsim(G, u(1:end-1));
y2 = lsim(Gt, diff(u));
yhat = y1+y2;
e = y(start+1:end)-yhat(start:end);
out = sum(e.^2) + theta(:)'*theta(:)*gamma;
end

