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