0
|
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()
|