# Discrete-Time Kalman Filter Demonstration (Python)
#
# A Python 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
#
# Additional comments/editing by M. R. Wirtzfeld (wirtzfeld@nca.uwo.ca)
# London, ON
# Saturday, March 31, 2007


import numpy
import pylab


# System - Constant voltage; no variation with time (stationary)
trueValue = -0.37727

# Measurement "noise" or observation "deviation" standard deviation
measurementNoise = 0.1

# Variance for time update of systems
processVariance = 1e-5

# Parameter Initialization
numberOfObservations = 500
arraySize = (numberOfObservations, )
observations = numpy.random.normal(trueValue, measurementNoise, size = arraySize)

# a priori Estimates
xhatminus = numpy.zeros(arraySize) # a priori estimate of trueValue
Pminus = numpy.zeros(arraySize)    # a priori error covariance estimate

# a posteriori Estimates
xhat = numpy.zeros(arraySize) # a posteriori estimate of trueValue
P = numpy.zeros(arraySize)    # a posteriori error covariance estimate

# gain or "blending" factor
K=numpy.zeros(arraySize)


R = 0.1**2 # Estimate of measurement variance; change to see effect


# Initial estimates/guesses of constant voltage and error covariance
xhat[0] = 0.0
P[0] = 1.0


# Kalman Recursion
for observation in range(1, numberOfObservations):

    # Time Update - "Prediction"
    xhatminus[observation] = xhat[observation - 1]  # State update

    Pminus[observation] = P[observation - 1] + processVariance  # a prior error covariance


    # Measurement Update - "Correction"
    K[observation] = Pminus[observation] / ( Pminus[observation] + R ) # gain or "blend"

    xhat[observation] = xhatminus[observation] + \
	K[observation]*(observations[observation] - xhatminus[observation])

    P[observation] = (1 - K[observation]) * Pminus[observation]



# Display Results
#
# True Value, Measurements, and a posteriori Estimate
pylab.figure()
pylab.plot(observations, 'k+', label = 'Noisy Measurements/Observations')
pylab.plot(xhat, 'b-', label = 'a posteriori Estimate')
pylab.axhline(trueValue, color = 'g', label = 'True Value')
pylab.legend()
pylab.xlabel('Iteration or Observation')
pylab.ylabel('Voltage')
#
# a priori Estimate
pylab.figure()
valid_iter = range(1, numberOfObservations) # Pminus not valid at step 0
pylab.plot(valid_iter, Pminus[valid_iter], label='a priori Error Estimate')
pylab.legend()
pylab.xlabel('Iteration or Observation')
pylab.ylabel('$(Voltage)^2$')
pylab.setp(pylab.gca(), 'ylim', [0,.01])
pylab.show()