% Discrete-Time Kalman Filter Demonstration (Matlab)
%
% A Matlab implementation of the example given on pages 11-15 of "An
% Introduction to the Kalman Filter" by Greg Welch and Gary Bishop,
% University of North Carolina at Chapel Hill, Department of Computer
% Science, TR 95-041
%
% http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
%
% Andrew D. Straw
%
% To demonstrate the impact of observation deviation or measurement noise,
% this script file will generate and plot results across several noise
% variance values.
%
% Additional comments/editing by M. R. Wirtzfeld (wirtzfeld@nca.uwo.ca)
% London, ON
% Wednesday, April 4, 2007


%% Environment
close all; clear all; clc;


%% Variable
% System response to estimate - Constant (stationary) voltage
trueValue = -0.375;

% Standard deviation of observations or measurement "noise"
measurementNoise = [0.001 0.01 0.1 1];
numberOfNoiseVariances = length(measurementNoise);

% Variance for time update of system
processVariance = 1e-5;

% Parameter initialization
numberOfObservations = 200;

% a-priori estimates
xhatminus = zeros(numberOfObservations, numberOfNoiseVariances); % a-priori estimate of true value
Pminus = zeros(numberOfObservations, numberOfNoiseVariances);    % a-priori error covariance estimate

% a-posteriori estimates
xhat = zeros(numberOfObservations, numberOfNoiseVariances); % a posteriori estimate of trueValue
P = zeros(numberOfObservations, numberOfNoiseVariances);    % a posteriori error covariance estimate

% Gain or "blending" factor
K = zeros(numberOfObservations, numberOfNoiseVariances);


%% Kalman Recursion
for noiseVarianceIndex = 1:1:numberOfNoiseVariances
    
    % Estimate of measurement variance; change to see effect
    R = measurementNoise(noiseVarianceIndex) ^ 2;
    
    observations = measurementNoise(noiseVarianceIndex) * randn(numberOfObservations, 1) + trueValue;

    % Initial estimates or "guesses" of constant voltage and error covariance
    xhat(1, noiseVarianceIndex) = 0.0;
    P(1, noiseVarianceIndex) = 1.0;

    for observationIndex = 2:1:numberOfObservations

        % Time update - "Prediction" of next system state; state update
        xhatminus(observationIndex, noiseVarianceIndex) = xhat(observationIndex - 1, noiseVarianceIndex);

        % a-priori error covariance estimate update
        Pminus(observationIndex, noiseVarianceIndex) = P(observationIndex - 1, noiseVarianceIndex) + processVariance;

        % Measurement update - "Correction" to state update; gain or "blend"
        %
        %
        K(observationIndex, noiseVarianceIndex) = Pminus(observationIndex, noiseVarianceIndex) /  ...
            ( Pminus(observationIndex, noiseVarianceIndex) + R );

        xhat(observationIndex, noiseVarianceIndex) = xhatminus(observationIndex, noiseVarianceIndex) + ...
            K(observationIndex, noiseVarianceIndex) * (observations(observationIndex) ...
            - xhatminus(observationIndex, noiseVarianceIndex));

        % a-posteriori error covariance estimate update
        P(observationIndex, noiseVarianceIndex) = (1 - K(observationIndex, noiseVarianceIndex)) * ...
            Pminus(observationIndex, noiseVarianceIndex);
    end;

end; % End noiseVarianceIndex For-loop


%% Display Results
for index = 1:1:numberOfNoiseVariances
    
    % True Value, measurements, and a-posteriori estimate
    figure(index); ...
        plot(observations, 'b+'); hold on;
        plot(xhat(:, index), '-r');
        plot(ones(numberOfObservations, 1) .* trueValue, 'g');
        legend('Noisy Measurements', 'a-posteriori Estimate', 'True Value');
        title(['Kalman Estimate of Constant Value, \sigma_N_o_i_s_e = ', num2str(measurementNoise(index)^2)]);
        xlabel('Iteration or Observation Index');
        ylabel('Voltage');

    % a-priori Estimate
    figure(index + 10); ...
        validIterations = 2:numberOfObservations;
        plot(validIterations, Pminus(validIterations, index));
        legend('a priori Error Estimate');
        xlabel('Iteration or Observation Index');
        ylabel('(Voltage)^2');
%         ylim([0 measurementNoise(index)^2]);

    % Kalman Gain
    figure(index + 20); ...
        plot(validIterations, K(validIterations, index));
        legend('Gain');
        xlabel('Iteration or Observation Index');
        ylabel('Dimensionless');

end; % End numberOfNoiseVariances For-loop