function [Xres, Pres, XRres] = Kalman_Loop()
    % Definizioni dei valori interni alle matrici del filtro
    e1 = 0.16^2;
    e2 = 0.1^2;
    q1 = 0.001^2;
    q2 = 0.001^2;
    r1 = 0.01^2;
    dt = 1;

    % Inizializzazione dello stato del filtro
    X = [0; 0];    
    P = [e1 0;0 e2];
    
    % Definizioni delle matrici del filtro
    PHI = [1 0.2*dt; 0 1];
    Q = [q1 0;0 q2];
    R = [r1];
    C = [1 0];

    % Inizializzazione del vettore dei risultati
    Xres = []; Pres = [];
    
    % Inizializzazione dello stato reale
    Xr = [0.04 0.025]; 
    
    % Loop per 1000 step
    for t=1:dt:1000

        % Lettura delle misure affette da rumore
        [Z, Xr] = Processo(Xr, dt);
    
        % Esecuzione del filtro
        [X, P] = KF(Z, X, P, PHI, C, Q, R);
    
        % Memorizzazione dei risultati
        XRres(t,:) = [Xr'];
        Xres(t,:) = [X'];
        Pres(t,:) = [P(1,1), P(1,2), P(2,1), P(2,2)];
    end
    
end

function [Xn, Pn] = KF(Z, X, P, PHI, C, Q, R)

% Aggiornamento dello stato
Xp = PHI * X;

% Aggiornamento della varianza dell'errore sullo stato
Pp = PHI * P * PHI' + Q;

% Calcolo della matrice di guadagno del filtro di Kalman
K = Pp * C' * inv(C *  Pp * C' + R);

% Aggiornamento della stima dello stato
Xn = Xp + K * (Z - C * Xp);

% Aggiornamento della varianza dei disturbi sullo stato e aggiornamento del ciclo
Pn = (eye(2) - K * C)* Pp;

end

function [Z, Xr] = Processo(Xr, dt)

x1 = Xr(1); 
x2 = Xr(2);

x1 = x1 + 0.2 * dt * x2 + 0.001*randn;
x2 = x2 + 0.001*randn;

Xr = [x1; x2];

Z = x1 + 0.01*randn;

end