def test_1d_array():
f1 = GHFilter (0, 0, 1, .8, .2)
f2 = GHFilter (array([0]), array([0]), 1, .8, .2)
str(f1)
str(f2)
# test both give same answers, and that we can
# use a scalar for the measurment
for i in range(1,10):
f1.update(i)
f2.update(i)
assert f1.x == f2.x[0]
assert f1.dx == f2.dx[0]
assert f1.VRF() == f2.VRF()
# test using an array for the measurement
s1 = Saver(f1)
s2 = Saver(f2)
for i in range(1,10):
f1.update(i)
f2.update(array([i]))
s1.save()
s2.save()
assert f1.x == f2.x[0]
assert f1.dx == f2.dx[0]
assert f1.VRF() == f2.VRF()
s1.to_array()
s2.to_array()
def test_lsq():
""" implements alternative version of first order Least Squares filter
using g-h filter formulation and uses it to check the output of the
LeastSquaresFilter class."""
gh = GHFilter(x=0, dx=0, dt=1, g=.5, h=0.02)
lsq = LeastSquaresFilterOriginal(dt=1, order=1)
lsq2 = LeastSquaresFilter(dt=1, order=1)
zs = [x + random.randn() for x in range(0, 100)]
xs = []
lsq_xs = []
for i, z in enumerate(zs):
g = 2 * (2 * i + 1) / ((i + 2) * (i + 1))
h = 6 / ((i + 2) * (i + 1))
x, dx = gh.update(z, g, h)
lx = lsq(z)
lsq_xs.append(lx)
x2 = lsq2.update(z)
assert near_equal(x2[0], lx, 1.e-13)
xs.append(x)
plt.plot(xs)
plt.plot(lsq_xs)
for x, y in zip(xs, lsq_xs):
r = x - y
assert r < 1.e-8
def tra_gh_filter(trajectory, v, avg_dis=152.5781, var_dis=327.7119):
point_list = []
for point in list(trajectory.coords):
point_list.append(Point(point))
point_num = len(point_list)
distance_list = []
for i in range(point_num - 1):
dis = point_list[i].distance(point_list[i + 1])
distance_list.append(dis)
error_list = []
for i in range(len(distance_list)):
if distance_list[i] > avg_dis + 3 * var_dis:
error_list.append(i)
error_point = []
if len(error_list) > 1:
for i in range(len(error_list) - 1):
error_dis_now = error_list[i]
error_dis_next = error_list[i + 1]
if error_dis_next == error_dis_now + 1:
error_index = error_dis_next
error_point.append(error_dis_next)
elif len(error_list) == 1:
if error_list[0] == point_num - 1:
error_index = point_num - 1
error_point.append(error_index)
x_0 = np.array([point_list[0].x, point_list[0].y])
dx_0 = np.array(v)
gh_f_tra = GHFilter(x=x_0, dx=dx_0, dt=10., g=.8, h=.2)
correct_point = [point_list[0]]
for i in range(1, len(point_list)):
if i in error_point:
gh_f_tra.g = 0
gh_f_tra.h = 0
else:
gh_f_tra.g = .8
gh_f_tra.h = .2
gh_f_tra.update(z=np.array([point_list[i].x, point_list[i].y]))
correct_point.append(gh_f_tra.x)
line_correct = LineString(correct_point)
return line_correct
def test_GHFilterOrder():
def fx(x):
return 2 * x + 1
f1 = GHFilterOrder(x0=array([0, 0]), dt=1, order=1, g=.6, h=.02)
f2 = GHFilter(x=0, dx=0, dt=1, g=.6, h=.02)
for i in range(100):
z = fx(i) + randn()
f1.update(z)
f2.update(z)
assert abs(f1.x[0] - f2.x) < 1.e-18
def __init__(self, face_rect, img):
self.face_rect = face_rect
self.last_time_eyes_open = time.time()
self.last_time_see_road = time.time()
self.names = {}
self.play_alarm = False
self.full_img = img
self.gaze_filter = GHFilter(x=0, dx=np.array([0]), dt=1, g=.6, h=.02)
self.skip_frame = 0
self.is_driver = False
self.start_yawn = False
self.life_counter = self.init_life_count
self.eye_counter = 0
self.yawn_count = 0
self.alive = True
self.born_time = time.time()
if audio:
self.music = vlc.MediaPlayer('rooster.mp3')
def test_lsq():
""" implements alternative version of first order Least Squares filter
using g-h filter formulation and uses it to check the output of the
LeastSquaresFilter class."""
global lsq, lsq2, xs, lsq_xs
gh = GHFilter(x=0, dx=0, dt=1, g=.5, h=0.02)
lsq = LeastSquaresFilterOriginal(dt=1, order=1)
lsq2 = LeastSquaresFilter(dt=1, order=1)
zs = [x+random.randn()*10 for x in range(0, 10000)]
# test __repr__ at least doesn't crash
try:
str(lsq2)
except:
assert False, "LeastSquaresFilter.__repr__ exception"
xs = []
lsq_xs = []
for i, z in enumerate(zs):
g = 2*(2*i + 1) / ((i+2)*(i+1))
h = 6 / ((i+2)*(i+1))
x, dx = gh.update(z, g, h)
lx = lsq(z)
lsq_xs.append(lx)
x2 = lsq2.update(z)
assert near_equal(x2[0], lx, 1.e-10), '{}, {}, {}'.format(
i, x2[0], lx)
xs.append(x)
plt.plot(xs)
plt.plot(lsq_xs)
for x, y in zip(xs, lsq_xs):
r = x-y
assert r < 1.e-8
def test_2d_array():
""" test using 2 independent variables for the
state variable.
"""
f = GHFilter(array([0, 1]), array([0, 0]), 1, .8, .2)
f0 = GHFilter(0, 0, 1, .8, .2)
f1 = GHFilter(1, 0, 1, .8, .2)
# test using scalar in update (not normal, but possible)
for i in range(1, 10):
f.update(i)
f0.update(i)
f1.update(i)
assert f.x[0] == f0.x
assert f.x[1] == f1.x
assert f.dx[0] == f0.dx
assert f.dx[1] == f1.dx
# test using array for update (typical scenario)
f = GHFilter(array([0, 1]), array([0, 0]), 1, .8, .2)
f0 = GHFilter(0, 0, 1, .8, .2)
f1 = GHFilter(1, 0, 1, .8, .2)
for i in range(1, 10):
f.update(array([i, i + 3]))
f0.update(i)
f1.update(i + 3)
assert f.x[0] == f0.x
assert f.x[1] == f1.x
assert f.dx[0] == f0.dx
assert f.dx[1] == f1.dx
assert f.VRF() == f0.VRF()
assert f.VRF() == f1.VRF()
####################################################
####################################################
# GH/Alpha betta filter - 2D attempt
####################################################
init_pos_guess = np.array([x_0,y_0])
init_vel_guess = np.array([1,1])
flag = True
while(flag):
# g,h = np.random.uniform(0,0.2),np.random.uniform(0,0.01)
g, h = 0.020282406381970187, 0.0003965804370818338
# Just noticed those were good, I'll keep the random uniform distributions up for tests later
print('g =',g, '\th =',h)
GH_air = GHFilter(x= init_pos_guess, dx = init_vel_guess, dt = 0.001, g = g, h= h) # g = .1, h = .005
Data = []
for i in range(count):
# print(i)
GH_air.update(z = np.array([x_data[i], y_data[i]])) # x, y
Data.append(GH_air.x)
# LATER, try and use GH_air.batch_filter([[],[],[],...,[]]) to update a whole batch of data instead of for loops
Data = np.array(Data)
print(count)
# print(Data)
fig = plt.figure()
#!/usr/bin/env python3
'''
H:向前移动
J:向上移动
K:向下移动
L:向后移动
'''
from filterpy.gh import GHFilter
f = GHFilter(x=0., dx=0., dt=1., g=.8, h=.2)
f.update(z=1.2)
print("f.update = ", f.update(z=1.2))
print("f.x = ", f.x, ";", "f.dx = ", f.dx)
print(f.update(z=2.1, g=.85, h=.15))
