def __init__(self, outputQueue, pathname, realtime):
super(json_import, self).__init__()
# external references
self.outputQueue = outputQueue
self.path = pathname
# internal references
self.gyrQueue = Queue()
self.accQueue = Queue()
self.magQueue = Queue()
self.sortedQueue = Queue()
# internal objects
self.gyrReader = json_reader(self.path + 'motion_gyro.json',
'gyroData', self.gyrQueue, realtime)
self.accReader = json_reader(self.path + 'motion_accel.json',
'accelData', self.accQueue, realtime)
self.magReader = json_reader(self.path + 'motion_magnet.json',
'magneticData', self.magQueue, realtime)
self.dtpSorter = dtp_sorter(self.gyrQueue, self.accQueue,
self.magQueue, self.sortedQueue)
self.dtpInterpolator = dtp_interpolator(self.sortedQueue,
self.outputQueue)
self.target = self.run()
def __init__(self, filename, realtime=False):
# user input
self.filename = filename
self.sampling_frequency = 20 # type 'None' if frequency should be determined from first two points
self.realtime = realtime
self.algoType = 1
'''
0 = Lee2015
1 = Deng2015
2 = Kang2018 (set freq to 20!)
3 = zerocrossing
4 = Brynes2016 (not working)
'''
self.filterType = 'gaussian'
'''
moving_average
gaussian
hann
kaiser_bessel
'''
# Internal queues for data flow
self.inputQueue = Queue()
self.preprocessQueue = Queue()
self.smoothedQueue = Queue()
self.orientationQueue = Queue()
self.stepCountQueue = Queue()
self.stepLengthQueue = Queue()
self.positionQueue = Queue()
self.simulationQueue = Queue()
def run_without_nans(self):
window = Queue()
while self.active:
df = pd.read_csv(self.filename, delimiter=',')
# convert absolute time to relative time in ms
time = pd.to_datetime(
df['loggingTime(txt)']).values.astype('datetime64[ms]')
time = (time - time[0]).astype(int)
# read in columns for gyro, acc and mag
gyr = df[self.column_names_gyro].values
acc = df[self.column_names_acc].values
mag = df[self.column_names_mag].values
# compute datapoints
for ii in range(time.size):
# first point
if ii == 0:
new_dtp = dtp(time[ii], a=acc[ii], g=gyr[ii], m=mag[ii])
new_dtp = dtp(time[ii], a=acc[ii], g=gyr[ii], m=mag[ii])
new_dtp.a_norm = vctr.norm(acc[ii])
new_dtp.g_norm = vctr.norm(gyr[ii])
self.outputQueue.enqueue(new_dtp)
self.outputQueue.enqueue('end')
self.active = False
def __init__(self, inputQueue, outputQueue):
super(Brynes2016, self).__init__()
self.target = self.run
self.inputQueue = inputQueue
self.outputQueue = outputQueue
self.typ = 999 #max_diff
self.windowSize = 5
self.threshold = 0
self.minNrOfPoints = 10
self.timeThreshold = 1 * 1000 # ms
self.n = 0
self.mean = 0
self.std = 0
self.intermediateQueue = Queue()
self.intermediateQueue2 = Queue()
def maxWait(self, arrival, service):
q = Queue(data=zip(arrival,service))
current_time = 0
max_waiting_time = 0
while not q.is_empty():
arrival, service = q.dequeue()
waiting_time = 0
if arrival > current_time:
current_time = arrival
waiting_time += current_time - arrival
current_time += service
max_waiting_time = max(max_waiting_time, waiting_time)
return max_waiting_time
def maxWait(self, arrival, service):
q = Queue(data=zip(arrival, service))
current_time = 0
max_waiting_time = 0
while not q.is_empty():
arrival, service = q.dequeue()
waiting_time = 0
if arrival > current_time:
current_time = arrival
waiting_time += current_time - arrival
current_time += service
max_waiting_time = max(max_waiting_time, waiting_time)
return max_waiting_time
def run(self):
window = Queue()
while self.active:
if len(self.inputQueue) != 0:
dtp = self.inputQueue.dequeue()
# Last point
if dtp == 'end':
# remove points left from midpoint
for ii in range(len(window)):
pop = window.dequeue()
if pop.time > self.outputQueue[-1].time:
self.outputQueue.enqueue(pop)
self.outputQueue.enqueue('end')
self.active = False
return
# remove GLOBAL_GRAVITY from acc_norm_smooth
dtp.a_norm_smooth -= GLOBAL_GRAVITY
window.enqueue(dtp)
# enqueue first points to output
if len(window) <= self.midPoint:
self.outputQueue.enqueue(dtp)
# compute if window midpoint is step
if len(window) == self.windowSize:
left = []
right = []
# divide the window in pre- and post datapoints
for i in range(self.midPoint):
left.append(window[i])
for i in range(self.midPoint, self.windowSize):
right.append(window[i])
# check if window has a zero crossing in the middle
if all(x.a_norm_smooth <= 0
for x in left) and all(x.a_norm_smooth > 0
for x in right):
# datapoint directly AFTER zero crossing is step
window[self.midPoint].isStep = True
# enqueue midpoint to output queue
self.outputQueue.enqueue(window[self.midPoint])
left.clear()
right.clear()
window.dequeue()
else:
time.sleep(0.05)
def postProcess(self):
self.windowQueue = Queue()
while True:
if len(self.intermediateQueue2) != 0:
# Get next data point
dp = self.intermediateQueue2.dequeue()
# Special case handling for end data point.
if dp == 'end':
self.lastPoint(self.intermediateQueue2)
return
# If we have less than 2 points in the queue, just enqueue
if dp.isStep:
if len(self.windowQueue) == 0:
self.windowQueue.enqueue(dp)
# self.enqueue(dp)
else:
if (self.windowQueue[0].isStep):
if (dp.time - self.windowQueue[0].time
) > self.timeThreshold:
for ii in range(len(self.windowQueue)):
pop = self.windowQueue.dequeue()
self.outputQueue.enqueue(pop)
elif dp.a_norm_smooth >= self.windowQueue[
0].a_norm_smooth:
self.windowQueue[0].isStep = False
for ii in range(len(self.windowQueue)):
pop = self.windowQueue.dequeue()
self.outputQueue.enqueue(pop)
else:
dp.isStep = False
else:
for ii in range(len(self.windowQueue)):
pop = self.windowQueue.dequeue()
self.outputQueue.enqueue(pop)
self.windowQueue.enqueue(dp)
def run(self):
window = Queue()
interpolation_count = 0
# if sampling frequency is given, convert to ms
if self.samplingFreq is not None:
self.samplingFreq /= 1000.
while self.active:
if len(self.inputQueue) != 0:
dp = self.inputQueue.dequeue()
# last data point
if dp == 'end':
self.outputQueue.enqueue('end')
self.active = False
return
window.enqueue(dp)
# convert unit G to m/s2
if self.convertG:
dp.convertGtoAcc()
# first data point
if len(window) == 1:
self.outputQueue.enqueue(dp)
# middle data points
if len(window) > 1:
# convert times from ms to s
time1 = window.queue[0].time
time2 = window.queue[1].time
# if no sampling frequency is given, use first two points to determine frequency
if self.samplingFreq is None:
self.samplingFreq = 1. / (time2 - time1)
# Check how many interpolation points COULD lie in between the timestamps
for i in range(
int(math.ceil(
(time2 - time1) * self.samplingFreq))):
interp_time = interpolation_count / self.samplingFreq
# If the interpolated time lies in this range, create the new data point and add it
if time1 <= interp_time < time2:
dp = linearInterp(window.queue[0], window.queue[1],
interp_time)
dp.a_norm = vctr.norm(dp.a)
dp.g_norm = vctr.norm(dp.g)
self.outputQueue.enqueue(dp)
interpolation_count += 1
# Pop oldest element
window.dequeue()
else:
time.sleep(0.05)
def run(self):
window = Queue()
while self.active:
if len(self.inputQueue) != 0:
dtp = self.inputQueue.dequeue()
# last point
if dtp == 'end':
for _ in range(len(window)):
pop = window.dequeue()
if pop.q_local is None:
pop.roll_pitch_yaw = quaternion_to_roll_pitch_yaw(self.q_global)
pop.q_local = np.array([1,0,0,0])
pop.q_global = self.q_global
self.outputQueue.enqueue(pop)
self.outputQueue.enqueue('end')
self.active = False
return
window.enqueue(dtp)
# implement Kalman Filter below!
# first point
if len(window) == 1:
dtp.q_local = np.array([1,0,0,0])
dtp.q_global = self.q_global
dtp.roll_pitch_yaw = np.array([0,0,0])
# intermediate points
else:
# average gyro between two points and time in s (convert from ms)
g_av = (window[1].g + window[0].g) / 2
dt = (window[1].time - window[0].time) / 1000
# compute local and global quaternion for single data point
q_local = vctr.normalise(angular_rate_to_quaternion_rotation(g_av, dt))
self.q_global = vctr.normalise(quaternion_product(self.q_global, q_local))
roll_pitch_yaw = quaternion_to_roll_pitch_yaw(self.q_global)
window[1].q_local = q_local
window[1].q_global = self.q_global
window[1].roll_pitch_yaw = roll_pitch_yaw
# append oldest point to output queue
pop = window.dequeue()
self.outputQueue.enqueue(pop)
else:
time.sleep(0.05)
def howMuch(self, arrival, departure, wage):
time_converter = lambda x: datetime.strptime(x, '%H:%M:%S')
arrival = map(time_converter, arrival)
departure = map(time_converter, departure)
q = Queue(zip(arrival, departure))
evening = datetime.strptime('18:00:00', '%H:%M:%S')
morning = datetime.strptime('06:00:00', '%H:%M:%S')
night_shifts = []
day_shifts = []
while not q.is_empty():
arr, dep = q.dequeue()
if arr < morning and dep < morning:
night_shifts.append((arr, dep))
elif arr < morning <= dep:
night_shifts.append((arr, morning))
q.enqueue((morning, dep))
elif morning <= arr < evening and morning <= dep < evening:
day_shifts.append((arr, dep))
elif arr < evening <= dep:
day_shifts.append((arr, evening))
q.enqueue((evening, dep))
else:
night_shifts.append((arr, dep))
pay = 0
pay += wage * sum(list(map(self.calculate_time_difference,
day_shifts)))
pay += wage * sum(
list(map(self.calculate_time_difference, night_shifts))) * 1.5
return pay
def run(self):
window = Queue()
while self.active:
if len(self.inputQueue.queue) != 0:
dtp = self.inputQueue.dequeue()
# last point in queue
if dtp == 'end':
# add remaining data points to queue (these are not filtered)
for ii in range(len(window)):
pop = window.dequeue()
pop.a_norm_smooth = pop.a_norm
pop.g_norm_smooth = pop.g_norm
self.outputQueue.enqueue(pop)
self.outputQueue.enqueue('end')
self.active = False
return
window.enqueue(dtp)
# if window size is reached:
if len(window.queue) == self.filterWindowSize:
# Compute average of filter window and store in midpoint
ssumAcc = 0
ssumGyr = 0
for i in range(self.filterWindowSize):
ssumAcc += window[i].a_norm * self.filter_window[i]
ssumGyr += window[i].g_norm * self.filter_window[i]
window[self.
midpoint].a_norm_smooth = ssumAcc / self.filter_sum
window[self.
midpoint].g_norm_smooth = ssumGyr / self.filter_sum
# store midpoint in output queue
self.outputQueue.enqueue(window[self.midpoint])
# dequeue oldest point in filter window
window.dequeue()
else:
time.sleep(0.05)
def maxDiff(self):
window = Queue()
while True:
if len(self.inputQueue) != 0:
# Get next data point
dp = self.inputQueue.dequeue()
# Special case handling for end data point.
if dp == 'end':
self.intermediateQueue.enqueue('end')
return
# Add data point to list and queue
window.enqueue(dp)
# Once we reach the window size, do some processing!
if len(window) == self.windowSize:
# Calculate peak score
midPoint = int(self.windowSize / 2)
maxDiffLeft = -100
maxDiffRight = -100
# Find max difference on left
for i in range(0, midPoint):
value = window[midPoint].a_norm_smooth - window[
i].a_norm_smooth
if value > maxDiffLeft:
maxDiffLeft = value
# Find max difference on right
for i in range(midPoint + 1, len(window)):
value = window[midPoint].a_norm_smooth - window[
i].a_norm_smooth
if value > maxDiffRight:
maxDiffRight = value
# Calculate peak score and create a new point
avg = (maxDiffRight + maxDiffLeft) / 2
window[midPoint].accNormNew = avg
self.intermediateQueue.enqueue(window[midPoint])
window.dequeue()
def panTompkins(self):
window = Queue()
while True:
if len(self.inputQueue) != 0:
# Get next data point
dp = self.inputQueue.dequeue()
# Special case handling for end data point.
if dp == 'end':
self.intermediateQueue.enqueue('end')
return
# Add data point to list and queue
window.enqueue(dp)
# Once we reach the window size, do some processing!
if len(window) == self.windowSize:
midPoint = int(self.windowSize / 2)
# Calculate mean of window
ssum = 0
for i in range(0, self.windowSize):
ssum += window[i].a_norm_smooth
mean = ssum / self.windowSize
new_mag = 0 if window[
midPoint].a_norm_smooth - mean < 0 else window[
midPoint].a_norm_smooth - mean
# Square it.
new_mag *= new_mag
# Calculate peak score and create a new point
window[midPoint].accNormNew = new_mag
self.intermediateQueue.enqueue(window[midPoint])
window.dequeue()
def run(self):
window = Queue()
step_window = Queue()
step_index = None
while self.active:
if len(self.inputQueue) != 0:
dtp = self.inputQueue.dequeue()
# last point
if dtp == 'end':
# confirm last step
if step_index is not None:
step_window[step_index].isStep = True
# add remaining data points to queue
for ii in range(len(step_window)):
pop = step_window.dequeue()
self.outputQueue.enqueue(pop)
self.outputQueue.enqueue(window[-1])
self.outputQueue.enqueue('end')
self.active = False
return
window.enqueue(dtp)
# first point
if len(window) == 1:
self.outputQueue.enqueue(dtp)
# intermediate points
if len(window) == 3:
self.accList.append(window[1].a_norm_smooth)
# INSERT extra statement here to consider points only above a certain 'active' threshold
if True: # window[1].accActive:
# state: 'peak', 'valley' or 'none'
state = self.detectState(window)
if state == 'peak':
# initial peak
if self.state_prev == 'none':
self.updatePeak(window[1])
# self.peaks.append(window[1])
# previous is valley AND new peak is above minimum time threshold
elif (self.state_prev == 'valley') and (
window[1].time - self.dtp_peak.time >
self.dt_peak):
self.updatePeak(window[1])
self.updateMean()
# previous is peak AND is within time threshold AND new peak has larger acc norm
elif (self.state_prev == 'peak') \
and (window[1].time - self.dtp_peak.time <= self.dt_peak) \
and (window[1].a_norm_smooth > self.dtp_peak.a_norm_smooth):
self.updatePeak(window[1], replace=True)
elif state == 'valley':
# previous is peak AND initial valley
if (self.state_prev
== 'peak') and (self.dtp_valley is None):
self.updateValley(window[1])
# potential step: store index in step_window
step_index = len(step_window)
#window[1].isStep = True
self.updateMean()
# previous is peak AND new valley is above minimum time threshold
elif (self.state_prev == 'peak') \
and (window[1].time - self.dtp_valley.time > self.dt_valley):
self.updateValley(window[1])
# potential step: store index in step_window
step_index = len(step_window)
#window[1].isStep = True
self.updateMean()
# previous is valley AND is within time threshold AND new valley has lower acc norm
elif (self.state_prev == 'valley') \
and (window[1].time - self.dtp_valley.time <= self.dt_valley) \
and (window[1].a_norm_smooth < self.dtp_valley.a_norm_smooth):
# change potential step index:
step_index = len(step_window)
self.updateValley(window[1], replace=True)
# compute standard deviation of acc:
self.std = np.std(np.array(self.accList))
# add data point to the step window
step_window.enqueue(window[1])
# if there is a potential step
if step_index is not None:
# if last valley is > time threshold, confirm step
if window[1].time - step_window[
step_index].time > self.dt_valley:
step_window[step_index].isStep = True
# append all data point up to and including step to output queue
for ii in range(step_index + 1):
pop = step_window.dequeue()
self.outputQueue.enqueue(pop)
step_index = None
# print(self.mean, self.std, self.dt_peak, self.dt_valley)
# reset counter if there has been no activity for some time
if window[
1].time - self.time_last_peak_or_valley > self.dt_max:
self.resetParams()
window.dequeue()
else:
time.sleep(0.05)
def run(self):
df = pd.read_csv(self.filename, delimiter=',')
# convert absolute time to relative time in ms
time_array = pd.to_datetime(
df['loggingTime(txt)']).values.astype('datetime64[ms]')
time_array = (time_array - time_array[0]).astype(int)
# read in columns for gyro, acc and mag
gyr = df[self.column_names_gyro].values
acc = df[self.column_names_acc].values
mag = df[self.column_names_mag].values
# initial data points
first_point = True
a_i = 0
g_i = 0
m_i = 0
window = Queue()
while self.active:
for kk in range(time_array.size):
a = acc[kk]
g = gyr[kk]
m = mag[kk]
dp = dtp(time_array[kk], a, g, m)
# last data point
if kk == time_array.size - 1:
a_final = window[a_i].a
g_final = window[g_i].g
m_final = window[m_i].m
for dp in window:
if pd.isna(dp.a[0]):
dp.a = a_final
if pd.isna(dp.g[0]):
dp.g = g_final
if pd.isna(dp.m[0]):
dp.m = m_final
dp.a_norm = vctr.norm(dp.a)
dp.g_norm = vctr.norm(dp.g)
self.outputQueue.enqueue(dp)
self.outputQueue.enqueue('end')
self.active = False
break
# first data point
if first_point:
if pd.isna(dp.a[0]):
dp.a = np.array([0., 0., 0.])
if pd.isna(dp.g[0]):
dp.g = np.array([0., 0., 0.])
if pd.isna(dp.m[0]):
dp.m = np.array([0., 0., 0.])
first_point = False
# intermediate datapoints
window.enqueue(dp)
if len(window) > 1:
# per dataype: if latest dp has an updated value, interpolate intermediate dps and update index
if not pd.isna(dp.a[0]):
slope = (dp.a - window[a_i].a) / (dp.time -
window[a_i].time)
for ii in range(a_i + 1, len(window)):
dt = window[ii].time - window[a_i].time
window[ii].a = window[a_i].a + slope * dt
a_i = len(window) - 1
if not pd.isna(dp.g[0]):
slope = (dp.g - window[g_i].g) / (dp.time -
window[g_i].time)
for ii in range(g_i + 1, len(window)):
dt = window[ii].time - window[g_i].time
window[ii].g = window[g_i].g + slope * dt
g_i = len(window) - 1
if not pd.isna(dp.m[0]):
slope = (dp.m - window[m_i].m) / (dp.time -
window[m_i].time)
for ii in range(m_i + 1, len(window)):
dt = window[ii].time - window[m_i].time
window[ii].m = window[m_i].m + slope * dt
m_i = len(window) - 1
# enqueue all datapoints below min index in window to the output queue
min_i = min([a_i, g_i, m_i])
for _ in range(min_i):
pop = window.dequeue()
pop.a_norm = vctr.norm(pop.a)
pop.g_norm = vctr.norm(pop.g)
self.outputQueue.enqueue(pop)
# recompute min indexes of window
a_i -= min_i
g_i -= min_i
m_i -= min_i
# if realtime = true, wait before next datapoint
if (kk != 0) and self.realtime:
time.sleep((time_array[kk] - time_array[kk - 1]) / 1000)
def run(self):
# initial data points
first_point = True
a_i = 0
g_i = 0
m_i = 0
window = Queue()
while self.active:
if len(self.inputQueue) != 0:
dp = self.inputQueue.dequeue()
# last data point
if dp == 'end':
a_final = window[a_i].a
g_final = window[g_i].g
m_final = window[m_i].m
for dp in window:
if dp.a is None:
dp.a = a_final
if dp.g is None:
dp.g = g_final
if dp.m is None:
dp.m = m_final
dp.a_norm = vctr.norm(dp.a)
dp.g_norm = vctr.norm(dp.g)
self.outputQueue.enqueue(dp)
self.outputQueue.enqueue('end')
self.active = False
break
# first data point
if first_point:
if dp.a is None:
dp.a = np.array([0., 0., 0.])
if dp.g is None:
dp.g = np.array([0., 0., 0.])
if dp.m is None:
dp.m = np.array([0., 0., 0.])
first_point = False
# intermediate datapoints
window.enqueue(dp)
if len(window) > 1:
# per dataype: if latest dp has an updated value, interpolate intermediate dps and update index
if dp.a is not None:
slope = (dp.a - window[a_i].a) / (dp.time -
window[a_i].time)
for ii in range(a_i + 1, len(window)):
dt = window[ii].time - window[a_i].time
window[ii].a = window[a_i].a + slope * dt
a_i = len(window) - 1
if dp.g is not None:
slope = (dp.g - window[g_i].g) / (dp.time -
window[g_i].time)
for ii in range(g_i + 1, len(window)):
dt = window[ii].time - window[g_i].time
window[ii].g = window[g_i].g + slope * dt
g_i = len(window) - 1
if dp.m is not None:
slope = (dp.m - window[m_i].m) / (dp.time -
window[m_i].time)
for ii in range(m_i + 1, len(window)):
dt = window[ii].time - window[m_i].time
window[ii].m = window[m_i].m + slope * dt
m_i = len(window) - 1
# enqueue all datapoints below min index in window to the output queue
min_i = min([a_i, g_i, m_i])
for _ in range(min_i):
pop = window.dequeue()
pop.a_norm = vctr.norm(pop.a)
pop.g_norm = vctr.norm(pop.g)
self.outputQueue.enqueue(pop)
# recompute min indexes of window
a_i -= min_i
g_i -= min_i
m_i -= min_i
def run(self):
'''two windows:
window is regular window that tracks if there are enough points,
step_window tracks all points since last potential step.
step_window resets if new step is detected'''
window = Queue()
step_window = Queue()
while self.active:
if len(self.inputQueue) != 0:
dtp = self.inputQueue.dequeue()
# Last point
if dtp == 'end':
# enqueue remaining part of step_window
for ii in range(len(step_window)):
pop = step_window.dequeue()
self.outputQueue.enqueue(pop)
# enqueue last data point of window
self.outputQueue.enqueue(window[-1])
self.outputQueue.enqueue('end')
self.active = False
return
window.enqueue(dtp)
# first point
if len(window) == 1:
self.outputQueue.enqueue(dtp)
# intermediate points
if len(window) == 3:
# Check for local peak AND above threshold
if window[1].a_norm_smooth > max([
window[0].a_norm_smooth, window[2].a_norm_smooth,
self.acc_threshold
]):
# first step identified
if not step_window[0].isStep:
window[1].isStep = True
# enqueue all points in step_window to output queue, empty step_window
for ii in range(len(step_window)):
pop = step_window.dequeue()
self.outputQueue.enqueue(pop)
# subsequent steps identified
else:
# check if dt > threshold
if (window[1].time -
step_window[0].time) > self.dt_threshold:
window[1].isStep = True
for ii in range(len(step_window)):
pop = step_window.dequeue()
self.outputQueue.enqueue(pop)
# check if new acc norm > acc norm of previous step
elif window[1].a_norm_smooth >= step_window[
0].a_norm_smooth:
window[1].isStep = True
step_window[0].isStep = False
for ii in range(len(step_window)):
pop = step_window.dequeue()
self.outputQueue.enqueue(pop)
# otherwise false positive
else:
pass
# enqueue middle data point to step_window
step_window.enqueue(window[1])
# remove oldest point from window
window.dequeue()
else:
time.sleep(0.05)
def run(self):
window = Queue()
walking_frequency = None
non_walking_windows = 0
walking_time = 0
step_residual = 0
while self.active:
if len(self.inputQueue) != 0:
dtp = self.inputQueue.dequeue()
# last point
if dtp == 'end':
# add remaining data points to queue
for ii in range(len(window)):
pop = window.dequeue()
self.outputQueue.enqueue(pop)
self.outputQueue.enqueue('end')
self.active = False
return
window.enqueue(dtp)
if len(window.queue) == self.window_size:
# get most sensitive gyro axis:
g_x = [x.g[0] for x in window]
g_y = [x.g[1] for x in window]
g_z = [x.g[2] for x in window]
g_sum = [sum(abs(x) for x in g_x),sum(abs(x) for x in g_y),sum(abs(x) for x in g_z)]
g_index = g_sum.index(max(g_sum))
g_target = [g_x, g_y, g_z][g_index]
# FFT of target gyro and separate into w_0 (below min freq) and w_c (within range)
f, w = self.fft(g_target)
w_0 = w[f < self.freq_min]
w_c = w[(f >= self.freq_min) & (f <= self.freq_max)]
f_c = f[(f >= self.freq_min) & (f <= self.freq_max)]
# First test: is there a higher average amplitude within range than below, and is this above 10
if np.average(w_c) > np.average([np.sum(w_0), self.w_threshold]):
p, r = self.get_max_freq(f_c,w_c)
if r.success:
# update total time walked
walking_time += self.sliding_distance_time
# get/update walking frequency for current time window
if walking_frequency is None:
walking_frequency = r.x
else:
walking_frequency = self.smoothing_weight * walking_frequency + (1 - self.smoothing_weight) * r.x
# determine steps within sliding distance interval
step_residual, nr_of_steps = math.modf(2 * walking_frequency * self.sliding_distance_time + step_residual)
step_position = int(self.sliding_distance / (2 * nr_of_steps))
for ii in range(int(nr_of_steps)):
window[2*ii*step_position].isStep = True
# else:
# print('no walking freq found')
else:
if walking_frequency is not None:
# MAX 1 windows without frequency to continue walking. After that, assume stop walking
if non_walking_windows < self.max_non_walking_windows:
# determine steps within sliding distance interval
step_residual, nr_of_steps = math.modf(2 * walking_frequency * self.sliding_distance_time + step_residual)
step_position = int(self.sliding_distance / (2 * nr_of_steps))
for ii in range(int(nr_of_steps)):
window[2 * ii * step_position].isStep = True
non_walking_windows += 1
else:
walking_frequency = None
step_residual = 0
non_walking_windows = 0
else:
step_residual = 0
non_walking_windows = 0
for ii in range(self.sliding_distance):
pop = window.dequeue()
pop.g_norm = g_target[ii]
pop.g_norm_smooth = g_target[ii]
self.outputQueue.enqueue(pop)
else:
time.sleep(0.05)
class Brynes2016(threatStructure):
def __init__(self, inputQueue, outputQueue):
super(Brynes2016, self).__init__()
self.target = self.run
self.inputQueue = inputQueue
self.outputQueue = outputQueue
self.typ = 999 #max_diff
self.windowSize = 5
self.threshold = 0
self.minNrOfPoints = 10
self.timeThreshold = 1 * 1000 # ms
self.n = 0
self.mean = 0
self.std = 0
self.intermediateQueue = Queue()
self.intermediateQueue2 = Queue()
def run(self):
# Part A: peak function:
if self.typ == 'max_diff':
self.maxDiff()
elif self.typ == 'mean_diff':
self.meanDiff()
elif self.typ == 'pan_tompkins':
self.panTompkins()
else:
raise Exception('Unknown peak scorer type: ' + self.typ)
# Part B: Scoring function:
self.peakDetect()
# Part C: Post-processing
self.postProcess()
def maxDiff(self):
window = Queue()
while True:
if len(self.inputQueue) != 0:
# Get next data point
dp = self.inputQueue.dequeue()
# Special case handling for end data point.
if dp == 'end':
self.intermediateQueue.enqueue('end')
return
# Add data point to list and queue
window.enqueue(dp)
# Once we reach the window size, do some processing!
if len(window) == self.windowSize:
# Calculate peak score
midPoint = int(self.windowSize / 2)
maxDiffLeft = -100
maxDiffRight = -100
# Find max difference on left
for i in range(0, midPoint):
value = window[midPoint].a_norm_smooth - window[
i].a_norm_smooth
if value > maxDiffLeft:
maxDiffLeft = value
# Find max difference on right
for i in range(midPoint + 1, len(window)):
value = window[midPoint].a_norm_smooth - window[
i].a_norm_smooth
if value > maxDiffRight:
maxDiffRight = value
# Calculate peak score and create a new point
avg = (maxDiffRight + maxDiffLeft) / 2
window[midPoint].accNormNew = avg
self.intermediateQueue.enqueue(window[midPoint])
window.dequeue()
def meanDiff(self):
window = Queue()
while True:
if len(self.inputQueue) != 0:
# Get next data point
dp = self.inputQueue.dequeue()
# Special case handling for end data point.
if dp == 'end':
self.intermediateQueue.enqueue('end')
return
# Add data point to list and queue
window.enqueue(dp)
# Once we reach the window size, do some processing!
if len(window) == self.windowSize:
# Calculate peak score
midPoint = int(self.windowSize / 2)
diffLeft = 0
diffRight = 0
# Find total difference on left
for i in range(0, midPoint):
value = window[midPoint].a_norm_smooth - window[
i].a_norm_smooth
diffLeft += value
# Find total difference on right
for i in range(midPoint + 1, len(window)):
value = window[midPoint].a_norm_smooth - window[
i].a_norm_smooth
diffRight += value
# Calculate peak score and create a new point
avg = (diffLeft + diffRight) / (self.windowSize - 1)
window[midPoint].accNormNew = avg
self.intermediateQueue.enqueue(window[midPoint])
window.dequeue()
def panTompkins(self):
window = Queue()
while True:
if len(self.inputQueue) != 0:
# Get next data point
dp = self.inputQueue.dequeue()
# Special case handling for end data point.
if dp == 'end':
self.intermediateQueue.enqueue('end')
return
# Add data point to list and queue
window.enqueue(dp)
# Once we reach the window size, do some processing!
if len(window) == self.windowSize:
midPoint = int(self.windowSize / 2)
# Calculate mean of window
ssum = 0
for i in range(0, self.windowSize):
ssum += window[i].a_norm_smooth
mean = ssum / self.windowSize
new_mag = 0 if window[
midPoint].a_norm_smooth - mean < 0 else window[
midPoint].a_norm_smooth - mean
# Square it.
new_mag *= new_mag
# Calculate peak score and create a new point
window[midPoint].accNormNew = new_mag
self.intermediateQueue.enqueue(window[midPoint])
window.dequeue()
def peakDetect(self):
while True:
if len(self.intermediateQueue) != 0:
# Get next data point
dp = self.intermediateQueue.dequeue()
# Special case handling for end data point.
if dp == 'end':
self.intermediateQueue2.enqueue('end')
return
# Update statistics
self.n += 1
if self.n == 1:
# First data point
self.mean = dp.a_norm_smooth
self.std = 0
elif self.n == 2:
# Second data point
o_mean = self.mean
self.mean = (dp.a_norm_smooth + self.mean) / 2.
self.std = math.sqrt(
(math.pow(dp.a_norm_smooth - self.mean, 2) +
math.pow(o_mean - self.mean, 2)) / 2)
else:
# Iteratively update mean and standard deviation
o_mean = self.mean
self.mean = (dp.a_norm_smooth +
(self.n - 1) * self.mean) / self.n
self.std = math.sqrt(
((self.n - 2) * math.pow(self.std, 2) /
(self.n - 1)) + math.pow(o_mean - self.mean, 2) +
math.pow(dp.a_norm_smooth - self.mean, 2) / self.n)
if self.n > self.minNrOfPoints:
# Check if we are above the threshold
if (dp.a_norm_smooth -
self.mean) > self.std * self.threshold:
# Declare this a peak
dp.isStep = True
self.intermediateQueue2.enqueue(dp)
def postProcess(self):
self.windowQueue = Queue()
while True:
if len(self.intermediateQueue2) != 0:
# Get next data point
dp = self.intermediateQueue2.dequeue()
# Special case handling for end data point.
if dp == 'end':
self.lastPoint(self.intermediateQueue2)
return
# If we have less than 2 points in the queue, just enqueue
if dp.isStep:
if len(self.windowQueue) == 0:
self.windowQueue.enqueue(dp)
# self.enqueue(dp)
else:
if (self.windowQueue[0].isStep):
if (dp.time - self.windowQueue[0].time
) > self.timeThreshold:
for ii in range(len(self.windowQueue)):
pop = self.windowQueue.dequeue()
self.outputQueue.enqueue(pop)
elif dp.a_norm_smooth >= self.windowQueue[
0].a_norm_smooth:
self.windowQueue[0].isStep = False
for ii in range(len(self.windowQueue)):
pop = self.windowQueue.dequeue()
self.outputQueue.enqueue(pop)
else:
dp.isStep = False
else:
for ii in range(len(self.windowQueue)):
pop = self.windowQueue.dequeue()
self.outputQueue.enqueue(pop)
self.windowQueue.enqueue(dp)
def lastPoint(self, window):
# add remaining data points to queue
for ii in range(len(window)):
pop = window.dequeue()
if pop.time > self.outputQueue[-1].time:
self.outputQueue.enqueue(pop)
self.outputQueue.enqueue('end')
