# http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
#这里是假设A=1，H=1的情况
iterTimes = 50
sz = (iterTimes,)
x = -0.37727 # truth value (typo in example at top of p. 13 calls this z)
z = np.random.normal(x,0.1,size=sz) # observations (normal about x, sigma=0.1)
Q = 1e-5 # process variance
xhat = np.zeros(sz)
P = np.zeros(sz)
xhatminus = np.zeros(sz)
Pminus=np.zeros(sz)
K=np.zeros(sz)
R = 0.1**2 # estimate of measurement variance, change to see effect
# intial guesses
xhat[0] = 0.0
P[0] = 1.0
for k in range(1,iterTimes):
# time update
xhatminus[k] = xhat[k-1] #X(k|k-1) = AX(k-1|k-1) + BU(k) + W(k),A=1,BU(k) = 0
Pminus[k] = P[k-1]+Q #P(k|k-1) = AP(k-1|k-1)A' + Q(k) ,A=1
# measurement update
K[k] = Pminus[k]/( Pminus[k]+R ) #Kg(k)=P(k|k-1)H'/[HP(k|k-1)H' + R],H=1
xhat[k] = xhatminus[k]+K[k]*(z[k]-xhatminus[k]) #X(k|k) = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)], H=1
P[k] = (1-K[k])*Pminus[k] #P(k|k) = (1 - Kg(k)H)P(k|k-1), H=1
plt.figure(num=1,figsize=(8,6))
plt.xlabel("Iteration",size=14)
plt.ylabel("Voltage",size=14)
plt.plot(z,'k+',label='noisy measurements') #测量值
plt.plot(xhat,'b-',label='a posteri estimate') #过滤后的值
plt.axhline(x,color='g',label='truth value') #系统值
plt.legend()
plt.savefig('kf.png',format='png')
plt.figure()
valid_iter = range(1,iterTimes) # Pminus not valid at step 0
plt.plot(valid_iter,Pminus[valid_iter],label='a priori error estimate')
plt.xlabel('Iteration')
plt.ylabel('$(Voltage)^2$')
plt.setp(plt.gca(),'ylim',[0,.01])
plt.savefig('kf1.png',format='png')
时间序列有三类重要的统计诊断，filter滤波,predict预测,smooth平滑。未来时刻用Kalman算法我们称之为预测，对当下的结果用Kalman算法我们称之为滤波，对过去的结果用Kalman算法我们称之为平滑。
