%%% 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 = 1;          % 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 ];
R = sigmaR^2 * [ 1 ];         % measurement noise covariance

P = zeros(2, 2, N);
xpred = zeros(2,N);        % state estimate
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

P(:,:,1) = 10*Q;

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


%% plot position: true (yellow); observations (blue); 
%%                predictions (green); estimates (red)
plot(x(1,:),'y');   % plot true state
hold on;
plot(z,'b');   % plot observations
plot(xpred(1,:),'g');  % plot prediction
plot(xhat(1,:),'r');    % plot state estimate
hold off;
pause;

%% close-up
axis([0 15 -4 4]);
pause;

%% plot velocity estimates: predictions (green); estimates(red)
hold off;
plot(xpred(2,:),'g');
hold on;
plot(xhat(2,:),'r');
hold off;
pause;

axis([0 50 -4 4]);
pause;

for i=1:N
    p(i) = P(1,1,i); 
    ppred(i) = Ppred(1,1,i); 
    v(i) = P(2,2,i); 
    vpred(i) = Ppred(2,2,i); 
end

plot(p,'b');
hold on;
plot(ppred,'g');
hold off;
pause;

plot(v,'b');
hold on;
plot(vpred,'g');
hold off;
pause;
