plt.rcParams['figure.figsize'] = (10, 8)
# -------------- Initial parameters for Filter 1 (Constant Level)----------------
number_of_iterations = 21 # time steps of kalman filter
f = 0 # fill rate of tank
initial_level = 1 # initial level of water tank
q = 0.0001 # process variance
r = 0.1 # estimate of measurement variance, change to see effect
isLinear = True # water level = previous time step water level + fill rate
# initial guesses
initial_xhat_guess = 0
initial_P_guess = 1000
filter1 = kf.KalmanFilter(number_of_iterations, f, initial_level, q, r,
initial_xhat_guess, initial_P_guess, isLinear)
plt.figure()
plt.plot(filter1.y_arr, 'r--*', label='measured')
plt.plot(filter1.xhat_arr, 'b-', label='estimated')
plt.plot(filter1.L, color='g', label='true value')
plt.legend()
plt.title('Estimate vs. time step', fontweight='bold')
plt.xlabel('Time Period')
plt.ylabel('Water Level')
# -------------- Initial parameters for Filter 2 (Constant Filling)----------------
number_of_iterations = 21 # time steps of kalman filter
f = 0.1 # fill rate of tank
initial_level = 0 # initial level of water tank
q = 0.00001 # process variance
def timerEvent(self, event):
# Read String from Main Serial Port
if self.flag:
try:
if self.ser.isOpen():
string = self.ser.readline()
if string != b'':
self.line = string.decode("utf-8", errors='replace')
# update string to graph for retrieve
if self.graph.isVisible(
) and not self.LTC_ser.isOpen():
self.graph.updateStream(self.line, ltc_data=None)
t = time.strftime('[%H:%M:%S]:>', time.localtime())
self.line = '>' + self.line[:-2]
self.tedit.append(self.line.replace('>', t))
self.tedit.moveCursor(QTextCursor.End)
# print(self.line)
except Exception as e:
print(e)
# Read ADC data from Arduino Board.
if self.LTC_ser.isOpen():
try:
# Set 'G' to get 30 bytes ADC data.
self.LTC_ser.write(b'G')
# Stop timer, in case ADC reading is out of timer loop.
self.timer.stop()
string = self.LTC_ser.readline()
if string != b'':
string = string.decode("utf-8", errors='replace')
# Convert string stream to List, sort list from small to high
data_list = eval(string)
filted_data = kalmanfilter.KalmanFilter(
data_list, n_iter=len(data_list))
self.filtering_data.append(filted_data[-1])
#sorted_list = sorted(data_list)
#print('data list=', tuple(filted_data), 'mean=', filted_data[-1])
with open('./log.csv', 'a+') as fl:
fl.write('Raw,' + str(data_list))
fl.write('\n')
fl.write('Filted,' + str(tuple(filted_data)))
fl.write('\n')
fl.write('Tracking,' + str(
tuple(
kalmanfilter.KalmanFilter(
self.filtering_data,
n_iter=len(self.filtering_data)))))
fl.write('\n')
# Get Compensation input
number = self.LTC_Comp_edit.text()
if number != '':
compensation = float(number)
else:
compensation = 0.0
# Data from ADC can be variant, this step shall consider t remove noise
#
median = filted_data[-1] + compensation
self.LTC_label.setText(str('%2.2f' % median))
self.graph.updateStream(self.line, ltc_data=median)
# Write data to CSV file and save it.
if self.line != '':
t = time.strftime('[%H:%M:%S]:>', time.localtime())
text = self.line.replace(
'>', t + ',' + str('%2.2f' % median) + ',')
if (self.file != ''):
with open(self.file, 'a+') as f:
f.write(text)
f.write('\n')
self.edit3.setText('Record:' + text)
except Exception as e:
print(e)
# take care to clean self.line each time before loop ends.
self.line = ''
self.timer.start(100, self)
# observation Y = C * x + v, v~N(0,R)
C = np.mat([[1, 0], [0, 1]])
R = np.mat([[2, 0], [0, 2]])
# 観測データの生成
for i in range(T):
x = A * x + B * u + np.random.multivariate_normal([0, 0], Q, 1).T
X.append(x)
y = C * x + np.random.multivariate_normal([0, 0], R, 1).T
Y.append(y)
# initialize
mu = np.mat([[0], [0]])
Sigma = np.mat([[0, 0], [0, 0]])
M = [mu] # 推定
# Kalman filter
kf = kf.KalmanFilter(A, B, u, Q, C, R, mu, Sigma)
for i in range(T):
mu, Sigma = kf.update(Y[i + 1])
M.append(mu)
# 描画
a, b = np.array(np.concatenate(X, axis=1))
plt.plot(a, b, 'rs-')
a, b = np.array(np.concatenate(M, axis=1))
plt.plot(a, b, 'bo-')
#plt.axis('equal')
xlabel("sample")
plt.show()