% define system parameters
a_1 = 0.5;
a_2 = -0.8;
a_3 = 0.1;
b_0 = 0.2;
b_1 = -1;
% real parameter vector
p_real = [a_1;a_2;a_3;b_0;b_1];

% define the dynamical system from there
Phi_fun = @(p) [p.'; 1,0,0,0,0; 0,1,0,0,0; 0,0,0,0,0; 0,0,0,1,0];
Phi = Phi_fun(p_real);
eig(Phi)
Gamma = [0;0;0;1;0];
G = [1;0;0;0;0];
C = [1,0,0,0,0];
d = 0;
x_0 = zeros(5,1);
% x_0 = randn(5,1)
x = x_0;

% define the simulation window & input function
k_array = 1:100;
k_max = k_array(end);
nu_array = 1*(-2 + 2*k_array/k_max + 1*sin(2*pi*k_array/k_max));
% initialize storage arrays
gamma_array = 0*nu_array;
S_array = zeros(k_max,length(p_real));

% initialize noise (uniform or with some cov matrix Q)
q_cov = 0.1;
uniform_or_random = 1; % 0..uniform cov; 1..random cov
if uniform_or_random == 0
    % want noise vectors with average per-entry variance = q_cov
    % For independent samples: covariance matrix = q_cov * I_k
    Q_array = q_cov * eye(k_max);
    % generate v_array ~ N(0, Q)
    % For diagonal Q, simple sqrt(q_cov)*randn is correct
    v_array = sqrt(q_cov) * randn(k_max,1);
else
    % define Q as a random, symmetric, positive definite matrix
    % but scale it so that the average diagonal (per-entry variance) equals q_cov
    A = randn(k_max,k_max);          % raw random matrix
    Q_raw = A' * A;                  % symmetric positive definite
    % compute scaling to set average variance to q_cov
    avg_var_raw = mean(diag(Q_raw));
    Q_array = (q_cov / avg_var_raw) * Q_raw;
    % compute a square root of Q and draw samples v_array ~ N(0,Q)
    [V,D] = eig(Q_array);              % Q = V*D*V'
    sqrtD = sqrt(D);             % element‑wise sqrt
    L = V * sqrtD;               % square‑root of Q
    v_array = L * randn(k_max,1);
end

% simulate the system
for i = 1:k_max
    gamma_array(i) = C*x + d*nu_array(i);
    x = Phi*x + Gamma*nu_array(i) + G*v_array(i);
    S_array(i, :) = x.';
end

% perform minimum-variance parameter estimate
R = 10*eye(length(p_real));
N = zeros(length(p_real),k_max-1);
S = S_array(1:end-1,:);
y = S_array(2:end,1);
Q = Q_array(2:end,2:end);
p_hat_MVE = (R*transpose(S)+N)/(S*R*transpose(S)+Q+S*N+transpose(N)*transpose(S)) * y
p_hat_LSQ = (S'*S)\S'*y

% plot inputs and outputs nicely
t = k_array;
figure()
% outputs (gamma) and inputs (nu)
yyaxis left
plot(t, nu_array, '-b', 'LineWidth', 1.5);
ylabel('Input \nu_k','Color','b');
set(gca,'YColor','b');
yyaxis right
plot(t, gamma_array, '-r', 'LineWidth', 1.5);
hold on
plot(3:(k_max+2),S_array*p_hat_MVE, '-r', 'LineWidth', 1.5, "LineStyle", '--');
plot(3:(k_max+2),S_array*p_hat_LSQ, '-r', 'LineWidth', 1.5, "LineStyle", ':');
ylabel('Output \gamma_k','Color','r');
set(gca,'YColor','r');

xlabel('Time step k');
title('System Input \nu_k and Output \gamma_k');
grid on;
legend({'\nu_k (input)','\gamma_k (output)'}, 'Location','best');
xlim([t(1) t(end)]);




