#!/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()