clear;
close all;
clc;

N = 1e6;

%% Linear Block

epsilon = -0.1 + 0.2 * rand(1,N);
u = 2 * rand(1, N) - 1;

% True Values
theta_star = [0.5; 0.3; 1];

z = zeros(1,N);
z(1) = epsilon(1);
for k=2:N
    z(k) = 0.5* z(k-1) + epsilon(k);
end

v = zeros(1,N);
v(1) = u(1);
v(2) = 0.5 * v(1) + u(2);
for k=3:N
    v(k) = theta_star(1) * v(k-1) + theta_star(2) * v(k-2) + theta_star(3) * u(k);
end

y = v + z;

%% Recursive Least Squares Method

% P [3x3 Matrix]
P = cell(1, N);
P{3} = diag(100*ones(3,1));
for k = 4:N
    P{k} = zeros(3, 3);
end

% theta_dash [ESTIMATED]
theta_dash = cell(1, N);
theta_dash{1} = [0;0;0];
for k = 2:N
    theta_dash{k} = zeros(3, 1);
end

for k = 3:N
    fi =  [y(k-1); y(k-2); u(k)];
   
    if k == 3
        P_prev = P{k};
        theta_prev = theta_dash{k};
    else
        P_prev = P{k-1};
        theta_prev = theta_dash{k-1};
    end
    
    P{k} = P_prev - ((P_prev * fi * (fi') * P_prev)/ (1 + (fi') * P_prev * fi));
    theta_dash{k} = theta_prev + P{k} * fi * (y(k) - fi' * theta_prev);
end

norm_theta_ls = zeros(1, N);

for k = 3:N
    norm_theta_ls(k) = norm(theta_dash{k}-theta_star);
end

%% Model Output

y_dash = zeros(1,N);
y_dash (1) = u(1);
y_dash (2) = theta_dash{1}(1) * y_dash (1) + u(2);
for k=3:N
    y_dash (k) = theta_dash{k}(1) * y_dash (k-1) + theta_dash{k}(2) * y_dash (k-2) + theta_dash{k}(3) * u(k);
end

%% Instrumental Variables Method

% P [3x3 Matrix]
P = cell(1, N);
P{3} = diag(100*ones(3,1));
for k = 4:N
    P{k} = zeros(3, 3);
end

% theta_dash [ESTIMATED]
theta_dash = cell(1, N);
theta_dash{1} = [0;0;0];
for k = 2:N
    theta_dash{k} = zeros(3, 1);
end

for k = 3:N
    psi =  [y_dash(k-1); y_dash(k-2); u(k)];
    fi =  [y(k-1); y(k-2); u(k)];
   
    if k == 3
        P_prev = P{k};
        theta_prev = theta_dash{k};
    else
        P_prev = P{k-1};
        theta_prev = theta_dash{k-1};
    end
    
    P{k} = P_prev - ((P_prev * psi * (fi') * P_prev)/ (1 + (fi') * P_prev * psi));
    theta_dash{k} = theta_prev + P{k} * psi * (y(k) - fi' * theta_prev);
end

norm_theta_iv = zeros(1, N);

for k = 3:N
    norm_theta_iv(k) = norm(theta_dash{k}-theta_star);
end

figure(1);
scatter(1:N, norm_theta_ls);
hold on;
grid on;
scatter(1:N, norm_theta_iv);
ylim([0, 0.02]);
legend("LS", "IV");