import numpy as np
import matplotlib.pyplot as plt
#这里是假设A=1,H=1, B=0的情况
# 故动态模型 X(k) = X(k-1) + 噪声
# Z(K) = X(k)
# 动态模型是一个常量
# intial parameters
n_iter = 50
sz = (n_iter,) # size of array
print('sz =',sz)
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)
print('z is',z)
Q = 1e-5 # process variance
# allocate space for arrays
xhat=np.zeros(sz) # a posteri estimate of x
P=np.zeros(sz) # a posteri error estimate
xhatminus=np.zeros(sz) # a priori estimate of x
Pminus=np.zeros(sz) # a priori error estimate
K=np.zeros(sz) # gain or blending factor
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,n_iter):
# 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()
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.xlabel('Iteration')
plt.ylabel('Voltage')
plt.show()
np.random.normal()的意思是一个正态分布,normal这里是正态的意思。
numpy.random.normal(loc=0,scale=1e-2,size=shape)
1、参数loc(float):正态分布的均值,对应着这个分布的中心。loc=0说明这一个以Y轴为对称轴的正态分布,
2、参数scale(float):正态分布的标准差,对应分布的宽度,scale越大,正态分布的曲线越矮胖,scale越小,曲线越高瘦。
3、参数size(int 或者整数元组):输出的值赋在shape里,默认为None。