Smoothing data using Kalman filter

kalman.py

# Kalman filtering
#  Prediction:
#    x_pre[k] = A*x[k-1] + B*u[k]
#    P_pre[k] = A*P[k-1]*A' + Q
#  Measurement update:
#    K[k] = P_pre[k]*H'*(H*P_pre[k]*H'+R)**(-1)
#    x[k] = x_pre[k] + K[k]*(z[k]-H*x_pre[k])
#    P[k] = (I-K[k]*H)*P_pre[k]
#
# Simplified with A=H=1, B=0, Q=0:
#  Prediction:
#    x_pre[k] = x[k-1]
#    P_pre[k] = P[k-1]
#  Measurement update:
#    K[k] = P_pre[k]/(P_pre[k]+R)
#    x[k] = x_pre[k] + K[k]*(z[k]-x_pre[k])
#    P[k] = (1-K[k])*P_pre[k]
#  

import numpy
SIZE=50
STDDEV=0.1
z = numpy.random.normal([3.14]*SIZE, STDDEV)
x = [None]*SIZE
x_pre = [None]*SIZE
K = [None]*SIZE
P = [None]*SIZE

x[0] = z[0]   # arbitrary
P[0] = STDDEV # arbitrary
for k in range(SIZE):
    if k==0:
        # initial estimate, arbitrary
        x[k] = z[k]
        P[k] = STDDEV
    else:
        # no prediction as it is just copy over from k-1 to x[k],P[k]
        # measurement update step below
        K[k] = P[k-1]/(P[k-1]+STDDEV)
        x[k] = x[k-1] + K[k] * (z[k]-x[k-1])
        P[k] = (1-K[k])*P[k-1]
    print("%.5f\t%.5f" % (z[k],x[k]))