% 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
%
% 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.1;

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

% Parameter initialization
numberOfObservations = 200;
observations = measurementNoise * randn(numberOfObservations, 1) + trueValue;

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

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

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

% Estimate of measurement variance; change to see effect
R = measurementNoise ^ 2;


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

for observationIndex = 2:1:numberOfObservations

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

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

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

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

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


%% Display Results
% True Value, measurements, and a-posteriori estimate
figure(10); ...
    plot(observations, 'b+'); hold on;
    plot(xhat, '-r');
    plot(ones(numberOfObservations, 1) .* trueValue, 'g');
    legend('Noisy Measurements', 'a-posteriori Estimate', 'True Value');
    title('Kalman Estimate of Constant Value');
    xlabel('Iteration or Observation Index');
    ylabel('Voltage');

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