%%% Matlab script to assess Kalman filter performance
%%% The script assumes the existence of a vector z of
%%% noise corrupted observations

N = length(z);          % number of Klamn filter iterations

Qfactor = 1;            % process noise mult factor
Rfactor = 10;          % measurement noise mult factor

delT = 1;               % time step
F = [ 1 delT            % update matrix  
      0   1  ];
H = [ 1 0 ];            % measurement matrix

sigmaQ = Qfactor*sqrt(0.01);  
sigmaR = Rfactor*sqrt(0.1);
Q = sigmaQ^2 * [ 1/3  1/2     % process noise covariance matrix
                 1/2  1 ];
P = 10*Q;
R = sigmaR^2 * [ 1 ];         % measurement noise covariance

xhat = zeros(2,N);        % state estimate
nu = zeros(1,N);          % innovation
S = zeros(1,N);           % innovation (co)variance
q = zeros(1,N);           % normalised innovation squared

for i=2:N
    [xpred, Ppred] = predict(xhat(:,i-1), P, F, Q);
    [nu(:,i), S(:,i)] = innovation(xpred, Ppred, z(i), H, R);
    [xhat(:,i), P] = innovation_update(xpred, Ppred, nu(:,i), S(:,i), H);
     q(:,i) = nu(:,i)'*inv(S(:,i))*nu(:,i);
end

sumQ = sum(q)       % determine Sum q which is Chi^2 on N d.o.f.
r = xcorr(nu);      % get autocorrealtion of innovation

plot(xhat(1,:));    % plot state estimate
pause;

plot(nu)            % plot innovation and 2sigma confidence interval
hold on;
plot(2*sqrt(S),'r');
plot(-2*sqrt(S),'r');
hold off;
pause;

plot(q);            % plot normalised innovation squared 
pause;

plot(r(N:2*N-1)/r(N));  % plot autocorrelation of innovation (normalised)

