1 #!/usr/bin/python

     2 # 14.4 & 14.3

     3 

     4 import pylab as pl

     5 

     6 def kal_ex(x, xline, M):

     7     y = x + pl.rand()

     8     Q = 0.0

     9     R = 1.0

    10     xhat_ = xline

    11     P_ = M + Q

    12     K = P_/(P_+R)

    13     P = (1-K)*P_

    14     xhat = xhat_ + K*(y-xhat_)

    15     return xhat, P, y

    16 

    17 x = 5 

    18 xhat = 2

    19 P = 1

    20 xvec = x 

    21 xhat_vec = xhat

    22 Pvec = P

    23 yvec = x

    24 for i in range(200):

    25     xline = xhat 

    26     M = P

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

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

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

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

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

    32 

    33 n = range(201)

    34 pl.plot(Pvec)

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

    36 pl.xlabel('n')

    37 pl.show()