#!/usr/bin/python

# 14.4 & 14.3

import pylab as pl

def kal_ex(x, xline, M):

    y = x + pl.rand()

    Q = 0.0

    R = 1.0

    xhat_ = xline

    P_ = M + Q

    K = P_/(P_+R)

    P = (1-K)*P_

    xhat = xhat_ + K*(y-xhat_)

    return xhat, P, y

x = 5 

xhat = 2

P = 1

xvec = x 

xhat_vec = xhat

Pvec = P

yvec = x

for i in range(200):

    xline = xhat 

    M = P

    xhat, P, y = kal_ex(x, xline, M)

    xvec = pl.vstack((xvec, x))

    xhat_vec = pl.vstack((xhat_vec, xhat))

    Pvec = pl.vstack((Pvec, P))

    yvec = pl.vstack((yvec, y))

n = range(201)

pl.plot(Pvec)

pl.plot(n, xhat_vec, n, yvec, n, xvec)

pl.xlabel('n')

pl.show()