% Test for checking the performance of our Kalman filter implementation
% Author: Saurabh Prasad
% Last modified: Thursday, November 1, 2005.
clear all;
error_avg = zeros(100,1)';

for loop = 1 : 100
    p = 4;
    P_old = eye(p,p);

    P = [0.03 0.06 0.70 0.05];
    x(1) = 0.1;    z(1)= x(1) + normrnd(0,0.02);
    x(2) = 0.2;    z(2)= x(2) + normrnd(0,0.02);
    x(3) = 0.5;    z(3)= x(3) + normrnd(0,0.02);
    x(4) = 0.8;    z(4)= x(4) + normrnd(0,0.02);
    % The state space model is a 4'th order AR process with an AWGN (0 mean, 0.02 variance)
    % driving noise. The measurement model is simple with variable noise
    % variance.
    for i = 5 : 100
        x(i)=P(1)*x(i-1)+P(2)*x(i-2)+P(3)*x(i-3)+P(4)*x(i-4) + normrnd(0,sqrt(0.02));
        z(i)= x(i) + normrnd(0,sqrt(0.02)); %Adjust the noise variance here
    end;
   
    temp = x; clear x; 
    F = [ 0, 1 , 0 , 0 ; 0, 0 , 1 , 0 ;0, 0 , 0 , 1 ; P(4), P(3), P(2), P(1)]; 
    G = zeros(1,4);
    G(1,4) = 1;
    G = G';
    H = G';

    ctr =1 ;
    x = z(1:4)'; 
    for i = 4 : 100
        Q = 0.02*eye(4,4);
        R = 0.02;
        X_estimate = x;
        % Predict the state vector and the error covariance matrix using
        % previous estimate of state vector and the error covariance matrix
        X_predict = F*X_estimate + (G*normrnd(0,0.02));
        P_new = F*P_old*F' + Q;    

        % Computing the Kalman Gain:          
        K = P_new*H'*inv(H*P_new*H'+ R);

        % Correcting the observation using the innovations process to generate
        % the 'filtered' estimate
        e(i) = z(i)-H*X_predict;  
        X_estimate = X_predict + (K*e(i));
        P_old = (eye(4,4)-K*H)*P_new;    

        % Filtering the observation, z 
        s = H*X_estimate;
        filtered_state(ctr:ctr+3) = X_estimate;
        x = X_estimate;
        ctr = ctr+1;
    end;
    filtered_state(1:4) = temp(1:4);
   

    for i = 1 : 100
        error(i) = ((temp(i) - filtered_state(i)))^2;
    end;
    error = error./max(abs((temp(:) - filtered_state(:))));
    error_avg = error+error_avg;
end;

subplot(2,1,1); 
plot(temp); hold on; plot(z,'r');ylabel('Amplitude'); title('Noise variance = 0.05');
hold on; plot(filtered_state,'g'); 
legend('Clean series' , 'Noisy Series' , 'Filtered Estimate');
error_avg = error_avg./100;
subplot(2,1,2); plot(error_avg); axis([0 100 0 1]);
xlabel('Time index'); ylabel('Normalized MSE'); title('MSE, Noise variance = 0.05');
