def KalmanFilter(z, n_iter=20):
# 这里是假设A=1，H=1的情况
# intial parameters
sz = (n_iter,) # size of array
# Q = 1e-5 # process variance
Q = 1e-6 # process variance
# allocate space for arrays
xhat = numpy.zeros(sz) # a posteri estimate of x
P = numpy.zeros(sz) # a posteri error estimate
xhatminus = numpy.zeros(sz) # a priori estimate of x
Pminus = numpy.zeros(sz) # a priori error estimate
K = numpy.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
A = 1
H = 1
for k in range(1, n_iter):
# time update
xhatminus[k] = A * xhat[k - 1] # X(k|k-1) = AX(k-1|k-1) + BU(k) + W(k),A=1,BU(k) = 0
Pminus[k] = A * 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] - H * xhatminus[k]) # X(k|k) = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)], H=1
P[k] = (1 - K[k] * H) * Pminus[k] # P(k|k) = (1 - Kg(k)H)P(k|k-1), H=1
return xhat
离线