def search(window, data):
from utils.math_functions.general_math import my_round
start = window[0]
end = window[1]
span = end - start
distance = 0
index = 0
for i, window in enumerate(data):
import statistics as stats
if window[2] > 1 / 3 * span:
start_dif = abs(start - window[0])
end_dif = abs(end - window[1])
temp = start_dif + end_dif
if distance < 1 / temp:
distance = 1 / temp
index = i
start = data[index][0]
end = data[index][1]
if (data[index][2] < span):
center = (start + end) / 2
start = center - span / 2
end = center + span / 2
start = my_round(start)
end = my_round(end)
return (start, end)
def gyro_troughs(angular_pos, gyro): """extract all troughs from gyroscope""" from utils.signal_analysis import find_peaks import matplotlib.pyplot as plt # keep only negative positions (troughs) neg_ang_pos = [0 if x > 0 else x for x in angular_pos] neg_ang_pos = list(map(abs, neg_ang_pos)) #plt.figure() #plt.plot(angular_pos['z']) #plt.figure() #plt.plot(neg_ang_pos['z']) #plt.show() # search for all troughs fs = 100 search_size = 40 min_dist = my_round(1/3*100) max_dist = my_round(1/0.5*100) # forward troughs troughs = find_peaks.forward(neg_ang_pos, search_size, min_dist, max_dist, fs) # backward troughs offset = list(reversed(neg_ang_pos)) backward_troughs = find_peaks.forward(offset, search_size, min_dist, max_dist, fs) offset = len(neg_ang_pos)-1 # used positive again GC? backward_troughs = list(reversed([offset-x for x in backward_troughs])) all_troughs = sorted(list(set(troughs + backward_troughs))) return all_troughs
def gyro_peaks(angular_pos, gyro): """extract all peaks from gyroscope""" # keep only positive positions (peaks) pos_ang_pos = [0 if x < 0 else x for x in angular_pos] # search for all peaks fs = 100 search_size = 40 min_dist = my_round(1/3*100) max_dist = my_round(1/0.5*100) # forward peaks peaks = find_peaks.forward(pos_ang_pos, search_size, min_dist, max_dist, fs) # backward peask temp = list(reversed(pos_ang_pos)) backward_peaks = find_peaks.forward(temp, search_size, min_dist, max_dist, fs) temp = len(pos_ang_pos)-1 backward_peaks = list(reversed([temp-x for x in backward_peaks])) #combine peaks all_peaks = sorted(list(set(peaks + backward_peaks))) return(all_peaks)
def accel_spikes(accel, gyro_peaks):
"""Find peaks and troughs of acceleration data"""
import matplotlib.pyplot as plt
# Calculations
fs = 100
Ny = fs / 2
f_cuts = [10 / Ny, 11 / Ny]
ripple_tol = [0.001, 0.1]
accel = lowpass(accel, f_cuts, fs, ripple_tol)
# Search for accelerometer peaks/troughs -direction
# Initialize approimate step ranges
fs = 100
search_size = 5
min_dist = my_round(1 / 4 * 100)
ma_dist = my_round(1 / 0.4 * 100)
all_accel_peaks = []
# search for acceleration peaks depending on gyroscope peak location
for i in range(0, len(gyro_peaks) - 1):
start_index = gyro_peaks[i]
end_index = gyro_peaks[i + 1]
search_size = 5
fs = 100
min_dist = my_round(1 / 4 * 100)
ma_dist = len(accel[start_index:end_index + 1])
accel_peak = find_peaks.forward(accel[start_index:end_index + 1],
search_size, min_dist, ma_dist, fs)
accel_peak = [x + start_index for x in accel_peak]
all_accel_peaks = all_accel_peaks + accel_peak
return (all_accel_peaks, accel)
def data_fil(data, h, pass_zero=False):
from scipy import signal
import statistics as stats
from utils.math_functions.general_math import my_round
# Filter the ecg signal
filtered_data = signal.lfilter(h, 1, data)
filtered_data = filtered_data.tolist()
# Invert the ecg signals, to filter the signal backwards
data_inverted = list(reversed(data))
filtered_inverted_data = signal.lfilter(h, 1, data_inverted).tolist()
# Reinvert the inverted filtered ecg signal, to right placement
reinverted_filtered_data = list(reversed(filtered_inverted_data))
# Find the offset value of the filter
filter_delay = my_round(len(h) / 2)
if (filter_delay > my_round(len(data) / 2)):
filter_delay = my_round(len(data) / 2)
adjusted_filtered_data = filtered_data[filter_delay - 1:]
# low pass filter settings....
# Read just the positioning due to phase offset
if (pass_zero == True):
adjusted_reinverted_filtered_data = \
[stats.mean(reinverted_filtered_data[0:-(filter_delay+20)])] \
* (filter_delay) + reinverted_filtered_data[0:-filter_delay]
cleaned_filtered_data = \
adjusted_filtered_data[0:my_round(len(adjusted_filtered_data) / 2)] + \
adjusted_reinverted_filtered_data[my_round(len(adjusted_filtered_data)/2):]
else:
adjusted_reinverted_filtered_data = \
[stats.mean(reinverted_filtered_data[0:-(filter_delay+20)])] \
* (filter_delay-1) + reinverted_filtered_data[0:-filter_delay+1]
cleaned_filtered_data = \
adjusted_filtered_data[0:my_round(len(adjusted_filtered_data) / 2)] + \
adjusted_reinverted_filtered_data[my_round(len(adjusted_filtered_data)/2):]
return cleaned_filtered_data
def max_jerk(accel, accel_peaks):
"""Use Jerk to find to, GC uncited"""
# find differnce between acceleration data-points
accel_first_diff = difference.first(accel, 1)
to_locations = []
temp = []
for i in range(0, len(accel_peaks)):
# find large window
start_index = accel_peaks[i]
if i == len(accel_peaks) - 1: # if last element
end_index = len(accel) - 11 # reverse index offset...
else:
end_index = my_round(
stats.mean([accel_peaks[i + 1],
accel_peaks[i]])) # index is center of peaks
# find locaton of highest jerk
sig = accel_first_diff[start_index:end_index] # half way to next peak
to_location = start_index + sig.index(max(sig))
temp.append(to_location)
# refine window
search_min = to_location - 5
search_max = to_location + 5 + 1
# find location of highest jerk
sig = accel[search_min:search_max]
to_location = search_min + sig.index(max(sig))
to_locations.append(
to_location) # plot the peaks and troughs of the data
return (to_locations, temp)
def extract(directory):
"""Find peaks and troughs of data"""
# initialize variables
pace = ['S', 'C', 'F']
coordinates = ['x', 'y', 'z']
# measure runtime
start_time = clocktime.time()
# Extract Patient Number
patient_number = directory[-6:]
print("Extracting data for patient:", patient_number)
# Open data_file, gravity_window, data_window
data, data_wdw, grav_wdw = open_files(directory)
if data == None:
print('No data for patient')
return
# Take out acceleration and gyroscope data from tailbone
accel_data = data['UR']['sensorData']['tailBone']['accel']['data']
gyro_data = data['UR']['sensorData']['tailBone']['gyro']['data']
#plt.figure()
#plt.plot(accel_data['z'])
#plt.figure()
#plt.plot(gyro_data['z'])
# Round all window coordinates
for p in pace:
for i in range(0,2):
data_wdw[p][i] = my_round(data_wdw[p][i])
grav_wdw[i] = my_round(grav_wdw[i])
print('Interval for motion data:', data_wdw)
print('Interval for gravity data:', grav_wdw)
# Check if not enough data
if data_wdw['F'][1] > len(gyro_data['x']):
print("Not enough gyro data", data_wdw['F'][1], len(gyro_data['x']))
return
# Initialize variables
accel = dict()
gyro = dict()
#plt.figure()
#plt.plot(accel_data['x'])
# Plot vertical lines at places of peaks and troughs
#plt.axvline(data_wdw['S'][0], color = 'g', linestyle = '--')
#plt.axvline(data_wdw['S'][1], color = 'b', linestyle = '--')
save_data = {}
# Iterate through slow, calm, fast paces
for k, p in enumerate(pace):
print("Extracting data for pace: ", p)
if (data_wdw['flag']['F'] == 0):
print('Not enough accel-data recorded')
continue
# cut data with windows
for w in coordinates:
accel[w] = accel_data[w][data_wdw[p][0]:data_wdw[p][1]]
gyro[w] = gyro_data[w][data_wdw[p][0]:data_wdw[p][1]]
gyro['sec'] = gyro_data['seconds'][data_wdw[p][0]:data_wdw[p][1]]
accel['sec'] = accel_data['seconds'][data_wdw[p][0]:data_wdw[p][1]]
# find gyro spikes
all_gyro_spikes, gyro_peaks, gyro_troughs, ang_pos = gyro_spikes(gyro)
speed = ['slow', 'calm', 'fast']
home = '../../figures/tailbone/'
home = os.path.join(home, patient_number)
output_dir = os.path.join(home, speed[k])
plt.plot(gyro['sec'], ang_pos)
plt.plot([gyro['sec'][x] for x in gyro_troughs], [ang_pos[x] for x in gyro_troughs], 'c^')
plt.plot([gyro['sec'][x] for x in gyro_peaks], [ang_pos[x] for x in gyro_peaks], 'co')
plt.legend(['angular_position', 'gyro_troughs', 'gyro_peaks'])
output = os.path.join(output_dir, 'gyro_spikes.pdf')
plt.savefig(output)
plt.close()
# NOTE: GC only finds peaks for accel, no troughs... weirdo
accel_peaks, accel_lp = accel_spikes(accel['x'][:], all_gyro_spikes)
#peaks = spikes(accel, gyro_peaks)
to_locations, temp = max_jerk(accel_lp[:], accel_peaks)
from utils.data_structure_functions import difference
accel_first_diff = difference.first(accel_lp,1)
plt.plot(accel['sec'], accel_lp)
plt.plot([accel['sec'][x] for x in to_locations], [accel_lp[x] for x in to_locations], 'k^')
plt.legend(['accel_diff', 'to_locations'])
#plt.show()
output = os.path.join(output_dir, 'to_locations.pdf')
plt.savefig(output)
plt.close()
plt.figure()
plt.plot(accel['sec'], accel_lp)
plt.plot([accel['sec'][x] for x in accel_peaks], [accel_lp[x] for x in accel_peaks], 'co')
plt.legend(['accel x', 'accel peaks'])
output = os.path.join(output_dir, 'accel_peaks.pdf')
plt.savefig(output)
plt.close()
HS, TO = right_left(accel_peaks, all_gyro_spikes, gyro_troughs, to_locations)
orientation = ['r', 'l']
linestyles = ['c*', 'm*', 'bo', 'ro']
legend1 = ['right HS', 'right TO', 'left HS', 'left TO']
plt.figure()
plt.plot(accel['sec'], accel_lp)
for c, i in enumerate(orientation, 1):
plt.plot([accel['sec'][x] for x in HS[i]],
[accel_lp[x] for x in HS[i]], linestyles[c*2-2])
plt.plot([accel['sec'][x] for x in TO[i]],\
[accel_lp[x] for x in TO[i]], linestyles[c*2-1])
plt.legend(['x accel', 'x diff'] + legend1)
output = os.path.join(output_dir, 'heel_toe.pdf')
plt.savefig(output)
plt.close()
save_data[p] = {}
save_data[p]['HS'] = HS
save_data[p] = {}
save_data[p]['TO'] = TO
with open(os.path.join(directory, 'uprite_hs_to.pkl'), 'wb') as afile:
pickle.dump(save_data, afile)
def extract(directory):
"""Create data windows on uprite data based on time recorded on zeno walkway"""
start_time = clocktime.time()
# Iterate trough every patient file
patient_number = directory[-6:]
print("Extracting data for patient:", patient_number)
"""Extract Pickle Data"""
# open file
uprite_pickle_file = os.path.join(directory, 'uprite_hs_to.pkl')
zeno_pickle_file = os.path.join(directory, 'zeno_hs_to.pkl')
with open(uprite_pickle_file, 'rb') as afile:
data = pickle.load(afile)
with open(zeno_pickle_file, 'rb') as afile:
zeno_data = pickle.load(afile)
# Check flags if there is no data
accel_flag = data['Flags']['tailBone']['accel']
gyro_flag = data['Flags']['tailBone']['gyro']
if (accel_flag == 0 or gyro_flag == 0):
print('No data recorded in patient:', patient_number)
return
#visual.print_keys(zeno_data,10)
padding = 0 # in seconds
interval, len_zeno, len_UR, window, offset = \
compare(data, zeno_data, padding)
pace = ['S', 'C', 'F']
for w in pace:
interval[w] = [my_round(x*100) for x in interval[w]]
"""Extract HS & TO for uprite system"""
"""First find all Troughs"""
# extract only raw tailbone data
accel_data = data['UR']['sensorData']['tailBone']['accel']['data']
gyro_data = data['UR']['sensorData']['tailBone']['gyro']['data']
# Change to only range of standing (where gravity is the only effect)
if interval['S'][0] > 0 and interval['S'][1] > 0:
fig = plt.figure(dpi = 300)
plt.subplots_adjust(wspace = 0.5, hspace = 0.5)
for c, w in enumerate(pace,1):
plt.subplot(220+c )
plt.plot(accel_data['x'][interval[w][0]:interval[w][1]])
plt.plot(accel_data['y'][interval[w][0]:interval[w][1]])
plt.plot(accel_data['z'][interval[w][0]:interval[w][1]])
plt.title(window[c-1])
plt.subplot(224)
plt.plot(accel_data['x'])
plt.plot(accel_data['y'])
plt.plot(accel_data['z'])
for w in pace:
plt.axvline(interval[w][0], color = 'b', linestyle = '--')
plt.axvline(interval[w][1], color = 'b', linestyle = '--')
plt.axvline(offset*100, color = 'r', linestyle = '--')
plt.axvline(interval['S'][0] +1600, color = 'r')
fig.suptitle(filename + ' || ' + str(offset))
#plt.title('Recorded Time |' + 'zeno:' + str(len_zeno) + 'UR:' + str(len_UR))
plt.show(block=True)
print('Successful run!')
print('-----------RUNTIME: %s second ----' % (clocktime.time() - start_time))
angular_pos[2]['z'], _, _, _ = filt.lowpass(angular_pos[1]['z'], cut, fs,
ripple_tol)
pos_ang_pos = {}
pos_ang_pos['z'] = angular_pos[2]['z']
pos_ang_pos['z'] = [0 if x < 0 else x for x in pos_ang_pos['z']]
neg_ang_pos = {}
neg_ang_pos['z'] = angular_pos[2]['z']
neg_ang_pos['z'] = [0 if x > 0 else x for x in neg_ang_pos['z']]
neg_ang_pos['z'] = list(map(abs, neg_ang_pos['z']))
# Search for gyroscope peaks/troughs z-direction
# Initializing approximate step ranges
search_size = 40
min_dist = my_round(1 / 3 * 100)
max_dist = my_round(1 / 0.5 * 100)
fs = 100
# search for all the peak
peaks,_,_,_ = find_peaks.forward(pos_ang_pos['z'], search_size, min_dist, \
max_dist, fs)
temp = list(reversed(pos_ang_pos['z']))
backward_peaks,_,_,_ = find_peaks.forward(temp, search_size, min_dist, \
max_dist, fs)
temp = len(pos_ang_pos['z']) - 1
backward_peaks = list(reversed([temp - x for x in backward_peaks]))
#combine peaks
all_peaks = sorted(list(set(peaks + backward_peaks)))
def extract(directory):
"""extract all the hs & to data from uprite sensor"""
start_time = clocktime.time() # record clocktime
save_data = {}
# Extract Patient Number
patient_name = directory[-6:]
print("Extracting data for patient:", patient_name)
# Open data_file, gravity_window, data_window
data, data_wdw, grav_wdw = open_files(directory)
# Take out acceleration and gyroscope data from tailbone
accel_all = data['UR']['sensorData']['tailBone']['accel']['data']
gyro_all = data['UR']['sensorData']['tailBone']['gyro']['data']
# Round all window coordinates
for p in pace:
for i in range(0, 2):
data_wdw[p][i] = my_round(data_wdw[p][i])
grav_wdw[i] = my_round(grav_wdw[i])
print('Interval for motion data:', data_wdw)
print('Interval for gravity data:', grav_wdw)
# Check if not enough data
if (data_wdw['flag']['F'] == 0):
print('Not enough accel-data recorded')
return
elif data_wdw['F'][1] > len(gyro_all['x']):
print("Not enough gyro data", data_wdw['F'][1], len(gyro_all['x']))
return
# Initialize variables
mean_accel = {}
accel_data = {}
gyro_data = {}
accel_data['seconds'] = accel_all['seconds']
gyro_data['seconds'] = gyro_all['seconds']
for w in coordinates:
mean_accel[w] = stats.mean(accel_all[w][grav_wdw[0]:grav_wdw[1]])
# Iterate through slow, calm, fast paces
for p in pace:
# Extract windowed data
for w in coordinates:
accel_data[w] = accel_all[w][data_wdw[p][0]:data_wdw[p][1]]
gyro_data[w] = gyro_all[w][data_wdw[p][0]:data_wdw[p][1]]
""" STRAIGHT GC (expect start_index/end_index) """
gravity_strength = math.sqrt(mean_accel['x']**2 + mean_accel['y']**2 + \
mean_accel['z']**2)
normalized_mean_accel = {}
normalized_mean_accel['x'] = mean_accel['x'] / gravity_strength
normalized_mean_accel['y'] = mean_accel['y'] / gravity_strength
normalized_mean_accel['z'] = mean_accel['z'] / gravity_strength
"""NU"""
normalized_gravity_vector = [normalized_mean_accel['x'], normalized_mean_accel['y'], \
normalized_mean_accel['z']]
# Key asumption here is that the gravity vector is [-1, 0, 0]
# so the x axis is upwards and downwards
# R_yzx
roll = 0 # math.atan(mean_accel['y']/mean_accel['z'])
pitch = math.atan(normalized_mean_accel['z'] /
normalized_mean_accel['x'])
yaw = math.atan(normalized_mean_accel['y']/math.sqrt(normalized_mean_accel['x']**2 + \
normalized_mean_accel['z']**2))
# Indexes used for analysis
start_index = 0
end_index = len(accel_data['x']) - 1
# Cut data to relevant data
accel = {}
accel['sec'] = accel_data['seconds'][start_index:(end_index)]
accel['x'] = accel_data['x'][start_index:(end_index)]
accel['y'] = accel_data['y'][start_index:(end_index)]
accel['z'] = accel_data['z'][start_index:(end_index)]
time = accel['sec']
# Put acceleration data through a low pass filter
# No gravity vector utilized yet.... don't know why it is called it yet...
f_cuts = [0.1 / 50, 0.7 / 50] # Passband & Stopband cutoffs
sampling_rate = 100
ripple_tol = [0.001, 0.1] # Passband & Stopband tolerances
gravity_accel = {}
gravity_accel['x'] = lowpass(accel_data['x'], f_cuts, sampling_rate, \
ripple_tol)
gravity_accel['y'] = lowpass(accel_data['y'], f_cuts, sampling_rate, \
ripple_tol)
gravity_accel['z'] = lowpass(accel_data['z'], f_cuts, sampling_rate, \
ripple_tol)
#plt.plot(gravity_accel['x'])
#plt.show()
# Change range of data....
gravity_accel['x'] = gravity_accel['x'][start_index:end_index]
gravity_accel['y'] = gravity_accel['y'][start_index:end_index]
gravity_accel['z'] = gravity_accel['z'][start_index:end_index]
gyro = {}
gyro['sec'] = gyro_data['seconds'][start_index:end_index]
gyro['x'] = gyro_data['x'][start_index:end_index]
gyro['y'] = gyro_data['y'][start_index:end_index]
gyro['z'] = gyro_data['z'][start_index:end_index]
# Labeled as high pass... but not high passed...
gyro_hpf = {}
gyro_hpf['x'] = gyro['x']
gyro_hpf['y'] = gyro['y']
gyro_hpf['z'] = gyro['z']
# Find the angular position of sensors
angular_pos = {}
angular_pos['x'] = integrate.IMU(gyro['sec'], gyro_hpf['x'])
angular_pos['x'] = [math.pi / 180 * x for x in angular_pos['x']]
angular_pos['y'] = integrate.IMU(gyro['sec'], gyro_hpf['y'])
angular_pos['y'] = [math.pi / 180 * x for x in angular_pos['y']]
angular_pos['z'] = integrate.IMU(gyro['sec'], gyro_hpf['z'])
angular_pos['z'] = [math.pi / 180 * x for x in angular_pos['z']]
# Gravity vector...
# A normalized gravity vector was made earlier.... not used...., but this is used...
gravity_vector = mean_accel
gravity_vector_holder = np.empty([3, len(accel['sec'])])
accel_delta = {}
accel_delta['x'] = [0] * len(accel['sec'])
accel_delta['y'] = [0] * len(accel['sec'])
accel_delta['z'] = [0] * len(accel['sec'])
accel_delta_vector_holder = np.empty([3, len(accel['sec'])])
accel2_delta = {}
accel2_delta['x'] = [0] * len(accel['sec'])
accel2_delta['y'] = [0] * len(accel['sec'])
accel2_delta['z'] = [0] * len(accel['sec'])
accel2_delta_vector_holder = np.empty([3, len(accel['sec'])])
earth_accel_delta_vector_holder = np.empty([3, len(accel['sec'])])
earth2_accel_delta_vector_holder = np.empty([3, len(accel['sec'])])
forward_earth_accel_delta_vector_holder = np.empty(
[3, len(accel['sec'])])
forward_position_delta_vector_holder = np.empty([3, len(accel['sec'])])
# Main function?
#plt.plot(gravity_accel['x'])
#plt.plot(accel['x'])
# Note this has a high executable time... need to optimize
for i in range(0, len(accel['sec'])):
# Calculate rotation
# These are the basic rotational matrixes from linar algebra
# Roll, pitch and yaw respectively
rotation = {}
rotation['x'] = np.empty([3, 3])
rotation['x'][0] = [1, 0, 0]
rotation['x'][1] = [0, math.cos(angular_pos['x'][i]), \
math.sin(angular_pos['x'][i])]
rotation['x'][2] = [0, -1*math.sin(angular_pos['x'][i]), \
math.cos(angular_pos['x'][i])]
rotation['y'] = np.empty([3, 3])
rotation['y'][0] = [math.cos(angular_pos['y'][i]), 0, \
-1*math.sin(angular_pos['y'][i])]
rotation['y'][1] = [0, 1, 0]
rotation['y'][2] = [math.sin(angular_pos['y'][i]), 0, \
math.cos(angular_pos['y'][i])]
rotation['z'] = np.empty([3, 3])
rotation['z'][0] = \
[math.cos(angular_pos['z'][i]), math.sin(angular_pos['z'][i]), 0]
rotation['z'][1] = \
[-1*math.sin(angular_pos['z'][i]), math.cos(angular_pos['z'][i]), 0]
rotation['z'][2] = [0, 0, 1]
# Why is it multiplied y to z to x? wiki says this is the order: z, y, x
# This order is due to where the gravity
# Matrix multiplication is not communitative
# Gravity vector is the constant gravity on acceleromter (3x1)
g_v = np.asarray(list(gravity_vector.values()))
gravity_vector_holder[:, i] = rotation['y'] @ rotation[
'z'] @ rotation['x'] @ g_v
# Difference between the measured acceleration and the direction of gravity
# Should be actual acceleration value :)
accel2_delta['x'][i] = accel['x'][i] - gravity_vector_holder[0, i]
accel2_delta['y'][i] = accel['y'][i] - gravity_vector_holder[1, i]
accel2_delta['z'][i] = accel['z'][i] - gravity_vector_holder[2, i]
accel2_delta_vector_holder[:,i] = \
[accel2_delta['x'][i], accel2_delta['y'][i], accel2_delta['z'][i]]
# Rotation with removed initial gravitational vector...
updated_rotation = {}
updated_rotation['x'] = np.empty([3, 3])
updated_rotation['x'][0] = [1, 0, 0]
updated_rotation['x'][1] = \
[0, math.cos(angular_pos['x'][i]), math.sin(angular_pos['x'][i])]
updated_rotation['x'][2] = \
[0, -1*math.sin(angular_pos['x'][i]), math.cos(angular_pos['x'][i])]
updated_rotation['y'] = np.empty([3, 3])
updated_rotation['y'][0] = [math.cos(angular_pos['y'][i]+pitch), 0, \
-1*math.sin(angular_pos['y'][i]+pitch)]
updated_rotation['y'][1] = [0, 1, 0]
updated_rotation['y'][2] = [math.sin(angular_pos['y'][i]+pitch), 0, \
math.cos(angular_pos['y'][i]+pitch)]
updated_rotation['z'] = np.empty([3, 3])
updated_rotation['z'][0] = [math.cos(angular_pos['z'][i]+yaw), \
math.sin(angular_pos['z'][i]+yaw), 0]
updated_rotation['z'][1] = [-1*math.sin(angular_pos['z'][i]+yaw), \
math.cos(angular_pos['z'][i]+yaw), 0]
updated_rotation['z'][2] = [0, 0, 1]
a = inv(updated_rotation['y'] @ updated_rotation['z']
@ updated_rotation['x'])
earth2_accel_delta_vector_holder[:,
i] = a @ accel2_delta_vector_holder[:,
i]
# Acceleration - Low passed acceleration
# Output: Noise from accelerometer
accel_delta['x'][i] = accel['x'][i] - gravity_accel['x'][i]
accel_delta['y'][i] = accel['y'][i] - gravity_accel['y'][i]
accel_delta['z'][i] = accel['z'][i] - gravity_accel['z'][i]
accel_delta_vector_holder[:,i] = \
[accel_delta['x'][i], accel_delta['y'][i], accel_delta['z'][i]]
updated_gravity_strength = math.sqrt(gravity_accel['x'][i]**2 + \
gravity_accel['y'][i]**2 + gravity_accel['z'][i]**2)
# Normalizing the low passed acceleration...
# This doesn't really make sense...
# you are scaling each acceleration movement, but...
# this means that there will be the same acceleration each entry... not true...
updated_normal_mean_accel = {}
updated_normal_mean_accel[
'x'] = gravity_accel['x'][i] / updated_gravity_strength
updated_normal_mean_accel[
'y'] = gravity_accel['y'][i] / updated_gravity_strength
updated_normal_mean_accel[
'z'] = gravity_accel['z'][i] / updated_gravity_strength
# Update pitch and yaw with low-passed data
new_ang = {}
new_ang['y'] = math.atan(updated_normal_mean_accel['z']/\
updated_normal_mean_accel['x']) # updated pitch
new_ang['z'] = math.atan(updated_normal_mean_accel['y']/math.sqrt(\
updated_normal_mean_accel['x']**2 + updated_normal_mean_accel['z']**2))
# updated yaw
# rotate so that the X axis points towards the earth
# weird because only pitch and yaw rotation
# was not used for initial acceleration data, only LP accel
updated_rotation['y'][0] = [
math.cos(new_ang['y']), 0, -1 * math.sin(new_ang['y'])
]
updated_rotation['y'][1] = [0, 1, 0]
updated_rotation['y'][2] = [
math.sin(new_ang['y']), 0,
math.cos(new_ang['y'])
]
updated_rotation['z'][0] = [
math.cos(new_ang['z']),
math.sin(new_ang['z']), 0
]
updated_rotation['z'][1] = [
-1 * math.sin(new_ang['z']),
math.cos(new_ang['z']), 0
]
updated_rotation['z'][2] = [0, 0, 1]
updated_rotation['x'] = np.identity(3)
# I have NO CLUE why we inverted the matrix multiplication
a = inv(updated_rotation['y'] @ updated_rotation['z']
@ updated_rotation['x'])
earth_accel_delta_vector_holder[:,
i] = a @ accel_delta_vector_holder[:,
i]
# rotate so that Z axis is direction of travel?
if (i > 1):
j = i + 1
# find position of non-filtered data...
# double integration to get position
coordinates = ['x', 'y', 'z']
earth_position_temp = {}
_, earth_position_temp['x'] = integrate.double(accel['sec'][:j], \
earth_accel_delta_vector_holder[0,:j])
earth_position_temp['y'] = integrate.IMU(accel['sec'][:j],\
integrate.IMU(accel['sec'][:j],earth_accel_delta_vector_holder[1,:j]))
earth_position_temp['z'] = integrate.IMU(accel['sec'][:j],\
integrate.IMU(accel['sec'][:j],earth_accel_delta_vector_holder[2,:j]))
position_strength = math.sqrt(earth_position_temp['x'][-1]**2 + \
earth_position_temp['y'][-1]**2 + earth_position_temp['z'][-1]**2)
# Normalize the distance moved? I don't see the point of this...
normalized_mean_position = {}
normalized_mean_position[
'x'] = earth_position_temp['x'][-1] / position_strength
normalized_mean_position[
'y'] = earth_position_temp['y'][-1] / position_strength
normalized_mean_position[
'z'] = earth_position_temp['z'][-1] / position_strength
# Where did this come from?
roll = math.atan(normalized_mean_position['y'] /
normalized_mean_position['z'])
pitch = math.atan(-1*normalized_mean_position['x']/math.sqrt(\
normalized_mean_position['y']**2 + normalized_mean_position['z']**2))
updated_rotation['x'][0] = [1, 0, 0]
updated_rotation['x'][1] = [0, math.cos(roll), math.sin(roll)]
updated_rotation['x'][2] = [
0, -1 * math.sin(roll),
math.cos(roll)
]
updated_rotation['y'][0] = [
math.cos(pitch), 0, -1 * math.sin(pitch)
]
updated_rotation['y'][1] = [0, 1, 0]
updated_rotation['y'][2] = [
math.sin(pitch), 0, math.cos(pitch)
]
updated_rotation['z'] = np.identity(3)
a = inv(updated_rotation['x'] @ updated_rotation['y']
@ updated_rotation['z'])
forward_earth_accel_delta_vector_holder[:,i] = a @ \
earth_accel_delta_vector_holder[:,i]
forward_position_delta_vector_holder[:,i] = a @[earth_position_temp['x'][-1],\
earth_position_temp['y'][-1], earth_position_temp['z'][-1]]
""" NU START """
# The hpf is only a subtraction of 0.18?
accel_pure_delta_Hpf = {}
accel_pure_delta_Hpf['z'] = [x - 0.18 for x in accel['z']]
# Calculate the velocity and the position through integration
pure_velocity = {}
pure_velocity['z'] = integrate.IMU(accel['sec'],
accel_pure_delta_Hpf['z'])
pure_position = {}
pure_position['z'] = integrate.IMU(accel['sec'], pure_velocity['z'])
""" NU END """
# Integrating the noise to get velocity and position noise
pure_delta_velocity, pure_delta_position = integrate.double(
accel['sec'], accel_delta)
# hpf is same as no filter
# Storing Noise?
accel_delta_hpf = {}
accel_delta_hpf['x'] = accel_delta['x']
accel_delta_hpf['y'] = accel_delta['y']
accel_delta_hpf['z'] = accel_delta['z']
#plt.plot(accel_delta['x'])
#plt.show()
# I double checked... it is noise... wtf
# Anyways, integrate to get velocity and acceleration
velocity, position = integrate.double(accel['sec'], accel_delta_hpf)
# hpf is same as no filter...
# Gravity vector MM with noise...
earth_accel_delta_hpf = {}
earth_accel_delta_hpf['x'] = earth_accel_delta_vector_holder[0, :]
earth_accel_delta_hpf['y'] = earth_accel_delta_vector_holder[1, :]
earth_accel_delta_hpf['z'] = earth_accel_delta_vector_holder[2, :]
# Find the position and velocity of noise MM gravity vector
earth_velocity, earth_position = integrate.double(
accel['sec'], earth_accel_delta_hpf)
earth_velocity['z'] = [4 * x for x in earth_velocity['z']]
earth_position['z'] = [4 * x for x in earth_position['z']]
# Why print this variable?
forward_earth_accel_delta_vector_holder[:, i]
# Different gravity vector alignment...
# x-axis aligned with gravity MM with noise
# Multiply each vector
forward_earth_accel_delta_hpf = {}
forward_earth_accel_delta_hpf[
'x'] = forward_earth_accel_delta_vector_holder[0, :]
forward_earth_accel_delta_hpf[
'y'] = forward_earth_accel_delta_vector_holder[1, :]
forward_earth_accel_delta_hpf[
'z'] = forward_earth_accel_delta_vector_holder[2, :]
# get velocity
forward_earth_velocity = integrate.single(
accel['sec'], forward_earth_accel_delta_hpf)
# multiply it by 4? wtf
forward_earth_velocity['z'] = [
4 * x for x in forward_earth_velocity['z']
]
# get position
forward_earth_position = integrate.single(accel['sec'],
forward_earth_velocity)
# hpf is same as regular again...
# Difference between measured acceleration and gravity
# This should be actual values :)
accel2_delta_hpf = deepcopy(accel2_delta)
# get the velocity and position
velocity2, position2 = integrate.double(accel['sec'], accel2_delta_hpf)
# same as previous, except added rotation from initial gravitational vector throu MM
coordinates = ['x', 'y', 'z']
earth2_accel_delta_hpf = {}
for c, w in enumerate(coordinates):
earth2_accel_delta_hpf[w] = earth2_accel_delta_vector_holder[c, :]
earth2_velocity, earth2_position = integrate.double(accel['sec'], \
earth2_accel_delta_hpf)
# figures of all data
legend1 = ['gravity removed', 'hpf', 'velocity', 'position']
# figure 1
plt.figure()
for axis in coordinates:
plt.plot(time, [9.81 * x for x in pure_delta_position[axis]])
plt.title('pure delta position')
plt.legend(coordinates)
# figure 2
plt.figure()
plt.plot(time, [9.81 * x for x in accel2_delta['z']])
plt.plot(time, [9.81 * x for x in accel2_delta_hpf['z']])
plt.plot(time, [9.81 * x for x in velocity2['z']])
plt.plot(time, [9.81 * x for x in position2['z']])
plt.title('z 2')
plt.legend(legend1)
# figures 3-5
for c, axis in enumerate(coordinates):
plt.figure()
plt.plot(time,
[9.81 * x for x in earth_accel_delta_vector_holder[c, :]])
plt.plot(time, [9.81 * x for x in earth_accel_delta_hpf[axis]])
plt.plot(time, [9.81 * x for x in earth_velocity[axis]])
plt.plot(time, [9.81 * x for x in earth_position[axis]])
plt.title(axis + ' earth')
plt.legend(legend1)
# figures 6-8
for c, axis in enumerate(coordinates):
plt.figure()
plt.plot(time, [
9.81 * x for x in forward_earth_accel_delta_vector_holder[c, :]
])
plt.plot(time,
[9.81 * x for x in forward_earth_accel_delta_hpf[axis]])
plt.plot(time, [9.81 * x for x in forward_earth_velocity[axis]])
plt.plot(time, [9.81 * x for x in forward_earth_position[axis]])
plt.title(axis + ' forward earth')
plt.legend(legend1)
# figures 9-11
for c, axis in enumerate(coordinates):
plt.figure()
plt.plot(
time,
[9.81 * x for x in earth2_accel_delta_vector_holder[c, :]])
plt.plot(time, [9.81 * x for x in earth2_accel_delta_hpf[axis]])
plt.plot(time, [9.81 * x for x in earth2_velocity[axis]])
plt.plot(time, [9.81 * x for x in earth2_position[axis]])
plt.title(axis + ' earth 2')
plt.legend(legend1)
# GC somehow initilizes tHS parameters....
HS = {}
stride_velocity = {}
mean_stride_velocity = {}
for i in range(0, 5):
HS[i] = {}
stride_velocity[i] = {}
mean_stride_velocity[i] = {}
HS[0]['r'] = [6.17, 7.27, 8.3500, 9.41, 10.48, 11.6]
HS[0]['l'] = [5.55, 6.7, 7.7900, 8.85, 9.92, 11.02]
HS[1]['r'] = [6.13, 7.22, 8.3100, 9.37, 10.45, 11.56]
HS[1]['l'] = [5.55, 6.68, 7.7600, 8.82, 9.89, 11]
HS[2]['r'] = [15.09, 16.20, 17.24, 18.29, 19.35]
HS[2]['l'] = [16.72, 17.76, 18.81]
HS[3]['r'] = [23.07, 24.14, 25.21, 26.26, 27.37]
HS[3]['l'] = [22.52, 23.61, 24.67, 25.74, 26.81]
HS[4]['r'] = [31.05, 32.20, 33.29, 34.37, 35.49]
HS[4]['l'] = [31.63, 32.74, 33.82, 34.92]
# Calculate stride velocity by dividing the position by time
# Heel strike position/time to heel strike
# Deleted bug
# Another place where HS and TO parameters are randomly found....
# These paraamters are n ever used... wtf...
TO = {}
HS.update({'r': [4.93, 6.17, 7.27, 8.35, 9.41, 10.48, 11.6, 12.78]})
TO['r'] = [5.69, 6.83, 7.92, 8.98, 10.05, 11.16, 12.3]
HS.update({'l': [5.55, 6.7, 7.79, 8.85, 9.92, 11.02, 12.17]})
TO['l'] = [5.14, 6.31, 7.41, 8.48, 9.56, 10.63, 11.75, 12.88]
# figure 12a
orientation = ['r', 'l']
legend1 = ['right HS', 'right TO', 'left HS', 'left TO']
linestyles = ['c*', 'm*', 'bo', 'ro']
plt.figure()
plt.subplot(211)
for i in range(0, 3):
plt.plot(time, earth_accel_delta_vector_holder[i, :])
for c, i in enumerate(orientation, 1):
plt.plot(HS[i], [0] * len(HS[i]), linestyles[(c * 2) - 2])
plt.plot(TO[i], [0] * len(TO[i]), linestyles[(c * 2) - 1])
plt.legend(coordinates + legend1)
# figure 12b
plt.subplot(212)
for axis in coordinates:
plt.plot(time, gyro[axis])
for c, i in enumerate(orientation, 1):
plt.plot(HS[i], [0] * len(HS[i]), linestyles[c * 2 - 2])
plt.plot(TO[i], [0] * len(TO[i]), linestyles[c * 2 - 1])
plt.legend(coordinates + legend1)
# Calculations
fs = 100
Ny = fs / 2
f_cuts = [10 / Ny, 11 / Ny]
earth_filt = {}
earth_filt['x'] = lowpass(earth_accel_delta_vector_holder[0,:], \
f_cuts, fs, ripple_tol)
gyro_filt = {}
gyro_filt['z'] = gyro['z']
# figure 13a
plt.figure()
plt.subplot(211)
plt.plot(time, earth_filt['x'])
plt.plot(time, difference.first(earth_filt['x'], 1))
for c, i in enumerate(orientation, 1):
plt.plot(HS[i], [0] * len(HS[i]), linestyles[c * 2 - 2])
plt.plot(TO[i], [0] * len(TO[i]), linestyles[c * 2 - 1])
plt.legend(['xaccel', 'x diff'] + legend1)
# figure 13b
plt.subplot(212)
plt.plot(gyro['sec'], [500 * x for x in angular_pos['z']])
plt.plot(gyro['sec'],
difference.first([500 * x for x in angular_pos['z']], 20))
plt.plot(gyro['sec'], gyro_filt['z'])
for c, i in enumerate(orientation, 1):
plt.plot(HS[i], [0] * len(HS[i]), linestyles[c * 2 - 2])
plt.plot(TO[i], [0] * len(TO[i]), linestyles[c * 2 - 1])
plt.legend(['z angle', 'z gyro', 'z diff'] + legend1)
#plt.show(block=False)
#plt.close('all')
# GC's second file: feature identification
# WHY THE F**K DID HE MAKE THIS INTO A SECOND FILE...
# by: abhay gupta :)
# rename angular_pos variable
temp = deepcopy(angular_pos)
angular_pos = {}
angular_pos[0] = deepcopy(temp)
# Find high-pass angular position for the z direction
fs = 100
cut = 0.5
gyro2_hpf = {}
gyro2_hpf['z'] = highpass(gyro['z'], fs, cut)
angular_pos[1] = {}
angular_pos[1]['z'] = integrate.IMU(gyro['sec'],
gyro2_hpf['z'],
units='rad')
# Find low-pass angular position for the z direction
fs = 100
Ny = fs / 2
cut = [5 / Ny, 6 / Ny]
angular_pos[2] = {}
angular_pos[2]['z'] = lowpass(angular_pos[1]['z'], cut, fs, ripple_tol)
pos_ang_pos = {}
pos_ang_pos['z'] = angular_pos[2]['z']
pos_ang_pos['z'] = [0 if x < 0 else x for x in pos_ang_pos['z']]
neg_ang_pos = {}
neg_ang_pos['z'] = angular_pos[2]['z']
neg_ang_pos['z'] = [0 if x > 0 else x for x in neg_ang_pos['z']]
neg_ang_pos['z'] = list(map(abs, neg_ang_pos['z']))
# Search for gyroscope peaks/troughs z-direction
# Initializing approximate step ranges
search_size = 40
min_dist = my_round(1 / 3 * 100)
max_dist = my_round(1 / 0.5 * 100)
fs = 100
# search for all the peak
peaks,_,_,_ = find_peaks.forward(pos_ang_pos['z'], search_size, min_dist, \
max_dist, fs)
temp = list(reversed(pos_ang_pos['z']))
backward_peaks,_,_,_ = find_peaks.forward(temp, search_size, min_dist, \
max_dist, fs)
temp = len(pos_ang_pos['z']) - 1
backward_peaks = list(reversed([temp - x for x in backward_peaks]))
#combine peaks
all_peaks = sorted(list(set(peaks + backward_peaks)))
#search for all the troughs
troughs,_,_,_ = find_peaks.forward(neg_ang_pos['z'], search_size, min_dist, \
max_dist, fs)
temp = list(reversed(neg_ang_pos['z']))
backward_troughs,_,_,_ = find_peaks.forward(temp, search_size, min_dist, \
max_dist, fs)
temp = len(pos_ang_pos['z']) - 1 # used positive again GC?
backward_troughs = list(reversed([temp - x for x in backward_troughs]))
all_troughs = sorted(list(set(troughs + backward_troughs)))
all_troughs_peaks = sorted(all_peaks + all_troughs)
# Search for accelerometer peaks/troughs x-direction
# Initialize approximate step ranges
search_size = 5
min_dist = my_round(1 / 4 * 100)
max_dist = my_round(1 / 0.4 * 100)
fs = 100
# find peaks x-direction
accel_peaks,_,_,_ = find_peaks.forward(earth_filt['x'], search_size, min_dist, \
max_dist, fs)
temp = list(reversed(earth_filt['x']))
backward_accel_peaks,_,_,_ = find_peaks.forward(temp, search_size, \
min_dist, max_dist, fs)
temp = len(earth_filt['x']) - 1
backward_accel_peaks = list(
reversed([temp - x for x in backward_accel_peaks]))
all_accel_peaks = sorted(list(set(accel_peaks + backward_accel_peaks)))
all_accel_peaks = [] # WHAT THE F**K GC CLEARING ALL THAT WORK
# why did GC skip the last index? wtf...
# Anyways.... calculates in-between peaks? smaller min dist...
for i in range(0, len(all_troughs_peaks) - 1):
start_index = all_troughs_peaks[i]
end_index = all_troughs_peaks[i + 1]
search_size = 5
fs = 100
min_dist = my_round(1 / 4 * 100)
max_dist = len(earth_filt['x'][start_index:end_index + 1])
accel_peak,_,_,_ = find_peaks.forward(earth_filt['x']\
[start_index:end_index+1], search_size, min_dist, max_dist, fs)
accel_peak = [x + start_index for x in accel_peak]
all_accel_peaks = all_accel_peaks + accel_peak
# find differnce between acceleration data-points
accel_first_diff = difference.first(earth_filt['x'], 1)
to_locations = []
for i in range(0, len(all_accel_peaks)):
start_index = all_accel_peaks[i]
if i == len(all_accel_peaks) - 1:
end_index = len(earth_filt['x']) - 11
else:
end_index = my_round(stats.mean([all_accel_peaks[i+1], \
all_accel_peaks[i]]))
if end_index <= start_index:
end_index = end_index # absolutely idiotic GC...
sig = accel_first_diff[start_index:end_index]
to_location = start_index + sig.index(
max(sig)) #GC subtracted 1 for no rs..
search_min = to_location - 5
search_max = to_location + 5 + 1
sig = earth_filt['x'][search_min:search_max]
to_location = search_min + sig.index(max(sig))
to_locations.append(
to_location) # plot the peaks and troughs of the data
# figure 14a
plt.figure()
plt.subplot(211)
plt.plot(gyro['sec'], angular_pos[2]['z'])
plt.plot([gyro['sec'][x] for x in all_peaks], [angular_pos[2]['z'][x] for x in \
all_peaks], 'gx')
plt.plot([gyro['sec'][x] for x in all_troughs], [angular_pos[2]['z'][x] for x \
in all_troughs], 'c^')
# Plot vertical lines at places of peaks and troughs
[
plt.axvline(gyro['sec'][x], color='g', linestyle='--')
for x in all_peaks
]
[
plt.axvline(gyro['sec'][x], color='b', linestyle='--')
for x in all_troughs
]
# figure 14b
plt.subplot(212)
plt.plot(gyro['sec'], earth_filt['x'])
plt.plot([accel['sec'][x] for x in all_accel_peaks], [earth_filt['x'][x] for x \
in all_accel_peaks], 'gx')
plt.plot([accel['sec'][x] for x in to_locations], [earth_filt['x'][x] for x in \
to_locations], 'k^')
plt.plot(accel['sec'], accel_first_diff)
# add plots of the hs and to... came randomly
for c, i in enumerate(orientation, 1):
plt.plot(HS[i], [0] * len(HS[i]), linestyles[c * 2 - 2])
plt.plot(TO[i], [0] * len(TO[i]), linestyles[c * 2 - 1])
plt.legend(['xaccel', 'accel_peaks', 'to location', 'x diff'] +
legend1)
# Plot vertical lines at places of peaks and troughs
[
plt.axvline(gyro['sec'][x], color='g', linestyle='--')
for x in all_peaks
]
[
plt.axvline(gyro['sec'][x], color='b', linestyle='--')
for x in all_troughs
]
patient_name = directory[-6:]
plt.savefig('../../docs/fig1_' + patient_name + p + '.pdf')
#^need to put legend on side of graph
# finds all HS and TO :)
HS['r'] = []
HS['l'] = []
TO['r'] = []
TO['l'] = []
for i in range(0, len(all_troughs)):
if (i != len(all_troughs) - 1):
first_trough = all_troughs[i]
second_trough = all_troughs[i + 1]
# find all the peaks between the troughs...
peak = [
x for x in all_peaks if first_trough < x < second_trough
]
if not peak:
continue
elif len(peak) > 1:
peak = peak[
0] # so only take in account first value? wtf...
# right leg movement
# find acceleration peaks between trough and peak
accel_peak = [x for x in all_accel_peaks \
if first_trough < x < peak]
if (not not accel_peaks and len(accel_peak) == 1):
accel_peak = accel_peak[0]
HS['r'].append(accel_peak)
to_location = [x for x in to_locations \
if first_trough < x < peak]
[TO['l'].append(x) for x in to_location]
else:
peak = peak[
0] # so only take in account first value? wtf...
# right leg movement (again?)
# find acceleration peaks between trough and peak
accel_peak = [x for x in all_accel_peaks \
if first_trough < x < peak]
if (not not accel_peaks and len(accel_peak) == 1):
accel_peak = accel_peak[0]
HS['r'].append(accel_peak)
to_location = [x for x in to_locations \
if first_trough < x < peak]
[TO['l'].append(x) for x in to_location]
# left leg movement
accel_peak = [
x for x in all_accel_peaks if peak < x < second_trough
]
if (not not accel_peak and len(accel_peak) == 1):
accel_peak = accel_peak[0]
HS['l'].append(accel_peak)
to_location = [
x for x in to_locations if peak < x < second_trough
]
[TO['r'].append(x) for x in to_location]
else:
first_trough = all_troughs[i]
peak = [x for x in all_peaks if x > first_trough]
if not not peak:
peak = peak[0]
else:
continue
# righ leg movement
accel_peak = [
x for x in all_accel_peaks if first_trough < x < peak
]
if (not not accel_peak and len(accel_peak) == 1):
accel_peak = accel_peak[0]
HS['r'].append(accel_peak)
to_location = [
x for x in to_locations if first_trough < x < peak
]
[TO['l'].append(x) for x in to_location]
## Analysis Plots
# figure 15a
plt.figure()
plt.subplot(211)
plt.plot(gyro['sec'], angular_pos[2]['z'])
plt.plot([gyro['sec'][x] for x in all_peaks], \
[angular_pos[2]['z'][x] for x in all_peaks], 'gx')
plt.plot([gyro['sec'][x] for x in all_troughs], \
[angular_pos[2]['z'][x] for x in all_troughs], 'c^')
plt.legend(['z angular position', 'peak', 'trough'])
# figure 15b
plt.subplot(212)
plt.plot(accel['sec'], earth_filt['x'])
plt.plot(accel['sec'], accel_first_diff)
for c, i in enumerate(orientation, 1):
plt.plot([accel['sec'][x] for x in HS[i]],
[earth_filt['x'][x] for x in HS[i]],
linestyles[c * 2 - 2])
plt.plot([accel['sec'][x] for x in TO[i]],\
[earth_filt['x'][x] for x in TO[i]], linestyles[c*2-1])
plt.legend(['xaccel', 'x diff'] + legend1)
# create output file with TO & HS data
head = ['HS right', 'HS left', 'TO right', 'TO left']
top = ['Data output for extracted HS and TO measurements']
save_data[p] = {}
save_data[p]['HS'] = {}
save_data[p]['HS'] = HS
save_data[p]['TO'] = {}
save_data[p]['TO'] = TO
plt.savefig('../../docs/' + patient_name + p + '.pdf')
with open(os.path.join(directory, 'uprite_hs_to.pkl'), 'wb') as afile:
pickle.dump(save_data, afile)
print("Completed HS & TO for patient: ", patient_name)
print('Successful run!')
print('-----------RUNTIME: %s second ----' %
(clocktime.time() - start_time))
def accel_spikes(accel, gyro_peaks):
"""Find peaks and troughs of acceleration data"""
f_cuts = [0.1 / 50, 0.7 / 50] # Passband & Stopband cutoffs
sampling_rate = 100
ripple_tol = [0.001, 0.1] # Passband & Stopband tolerances
# create gravity vector by lowpass filtering acceleraiton data
gravity_vector = {}
gravity_vector['x'] = lowpass(accel['x'], f_cuts, sampling_rate,
ripple_tol)
gravity_vector['y'] = lowpass(accel['y'], f_cuts, sampling_rate,
ripple_tol)
gravity_vector['z'] = lowpass(accel['z'], f_cuts, sampling_rate,
ripple_tol)
# initialize variables
accel_delta = {}
accel_delta['x'] = [0] * len(accel['sec'])
accel_delta['y'] = [0] * len(accel['sec'])
accel_delta['z'] = [0] * len(accel['sec'])
accel_delta_vector = np.empty([3, len(accel['sec'])])
earth_accel_delta_vector = np.empty([3, len(accel['sec'])])
rotation = {}
rotation['x'] = np.empty([3, 3])
rotation['y'] = np.empty([3, 3])
rotation['z'] = np.empty([3, 3])
for i in range(0, len(accel['sec'])):
# Rotation with removed initial gravitational vector...
# Remove gravity from acceleration vector
accel_delta['x'][i] = accel['x'][i] - gravity_vector['x'][i]
accel_delta['y'][i] = accel['y'][i] - gravity_vector['y'][i]
accel_delta['z'][i] = accel['z'][i] - gravity_vector['z'][i]
accel_delta_vector[:, i] = [
accel_delta['x'][i], accel_delta['y'][i], accel_delta['z'][i]
]
# normalize gravity vector
gravity_strength = math.sqrt(gravity_vector['x'][i]**2 +
gravity_vector['y'][i]**2 +
gravity_vector['z'][i]**2)
norm_grav = {}
norm_grav['x'] = gravity_vector['x'][i] / gravity_strength
norm_grav['y'] = gravity_vector['y'][i] / gravity_strength
norm_grav['z'] = gravity_vector['z'][i] / gravity_strength
# Create gravity based rotational vectors (allows to calibrate gravity as you walk)
grav_ang = {}
grav_ang['y'] = math.atan(norm_grav['z'] /
norm_grav['x']) # updated pitch
grav_ang['z'] = math.atan(
norm_grav['y'] / math.sqrt(norm_grav['x']**2 + norm_grav['z']**2))
rotation['x'] = np.identity(3)
rotation['y'][0] = [
math.cos(grav_ang['y']), 0, -1 * math.sin(grav_ang['y'])
]
rotation['y'][1] = [0, 1, 0]
rotation['y'][2] = [
math.sin(grav_ang['y']), 0,
math.cos(grav_ang['y'])
]
rotation['z'][0] = [
math.cos(grav_ang['z']),
math.sin(grav_ang['z']), 0
]
rotation['z'][1] = [
-1 * math.sin(grav_ang['z']),
math.cos(grav_ang['z']), 0
]
rotation['z'][2] = [0, 0, 1]
# I have NO CLUE why we inverted the matrix multiplication
a = inv(rotation['y'] @ rotation['z'] @ rotation['x'])
earth_accel_delta_vector[:, i] = a @ accel_delta_vector[:, i]
# Calculations
fs = 100
Ny = fs / 2
f_cuts = [10 / Ny, 11 / Ny]
ripple_tol = [0.001, 0.1]
earth_filt = {}
earth_filt['x'] = lowpass(earth_accel_delta_vector[0, :], f_cuts, fs,
ripple_tol)
# Search for accelerometer peaks/troughs x-direction
# Initialize approximate step ranges
fs = 100
search_size = 5
min_dist = my_round(1 / 4 * 100)
max_dist = my_round(1 / 0.4 * 100)
all_accel_peaks = []
# search for acceleration peaks depending on gyroscope peak location
for i in range(0, len(gyro_peaks) - 1):
start_index = gyro_peaks[i]
end_index = gyro_peaks[i + 1]
search_size = 5
fs = 100
min_dist = my_round(1 / 4 * 100)
max_dist = len(earth_filt['x'][start_index:end_index + 1])
accel_peak = find_peaks.forward(
earth_filt['x'][start_index:end_index + 1], search_size, min_dist,
max_dist, fs)
accel_peak = [x + start_index for x in accel_peak]
all_accel_peaks.append(accel_peak)
return (all_accel_peaks)
def extract(directory):
"""extract all the hs & to data from uprite sensor"""
global coordinates
global orienation
global pace
start_time = clocktime.time() # record clocktime
save_data = {}
# Extract Patient Number
patient_name = directory[-6:]
print("Extracting data for patient:", patient_name)
# Open data_file, gravity_window, data_window
data, data_wdw, grav_wdw = open_files(directory)
# Take out acceleration and gyroscope data from tailbone
accel_all = data['UR']['sensorData']['tailBone']['accel']['data']
gyro_all = data['UR']['sensorData']['tailBone']['gyro']['data']
# Round all window coordinates
for p in pace:
for i in range(0, 2):
data_wdw[p][i] = my_round(data_wdw[p][i])
grav_wdw[i] = my_round(grav_wdw[i])
print('Interval for motion data:', data_wdw)
print('Interval for gravity data:', grav_wdw)
# Check if not enough data
if (data_wdw['flag']['F'] == 0):
print('Not enough accel-data recorded')
return
elif data_wdw['F'][1] > len(gyro_all['x']):
print("Not enough gyro data", data_wdw['F'][1], len(gyro_all['x']))
return
### MAY BE UNNEC
mean_accel = dict()
for w in coordinates: #gravity vector
mean_accel[w] = stats.mean(accel_all[w][grav_wdw[0]:grav_wdw[1]])
# Initialize variables
accel = dict()
gyro = dict()
# Iterate through slow, calm, fast paces
for p in pace:
# cut data with windows
for w in coordinates:
accel[w] = accel_all[w][data_wdw[p][0]:data_wdw[p][1]]
gyro[w] = gyro_all[w][data_wdw[p][0]:data_wdw[p][1]]
gyro['sec'] = gyro_all['seconds'][data_wdw[p][0]:data_wdw[p][1]]
accel['sec'] = accel_all['seconds'][data_wdw[p][0]:data_wdw[p][1]]
####
# rename angular_pos variable
angular_pos = {}
# Find high-pass angular position for the z direction
fs = 100
cut = 0.5
gyro2_hpf = {}
gyro2_hpf['z'] = highpass(gyro['z'], fs, cut)
angular_pos[1] = {}
angular_pos[1]['z'] = integrate.IMU(gyro['sec'],
gyro2_hpf['z'],
units='rad')
# Find low-pass angular position for the z direction
fs = 100
Ny = fs / 2
cut = [5 / Ny, 6 / Ny]
angular_pos[2] = {}
angular_pos[2]['z'] = lowpass(angular_pos[1]['z'], cut, fs, ripple_tol)
neg_ang_pos = {}
neg_ang_pos['z'] = angular_pos[2]['z']
neg_ang_pos['z'] = [0 if x > 0 else x for x in neg_ang_pos['z']]
neg_ang_pos['z'] = list(map(abs, neg_ang_pos['z']))
# Search for gyroscope peaks/troughs z-direction
# Initializing approximate step ranges
search_size = 40
min_dist = my_round(1 / 3 * 100)
max_dist = my_round(1 / 0.5 * 100)
fs = 100
#search for all the troughs
troughs,_,_,_ = find_peaks.forward(neg_ang_pos['z'], search_size, min_dist, \
max_dist, fs)
temp = list(reversed(neg_ang_pos['z']))
backward_troughs,_,_,_ = find_peaks.forward(temp, search_size, min_dist, \
max_dist, fs)
temp = len(neg_ang_pos['z']) - 1 # used positive again GC?
backward_troughs = list(reversed([temp - x for x in backward_troughs]))
all_troughs = sorted(list(set(troughs + backward_troughs)))
plt.plot(gyro['sec'], angular_pos[2]['z'])
plt.plot([gyro['sec'][x] for x in all_troughs], [angular_pos[2]['z'][x] for x \
in all_troughs], 'c^')
plt.show()
####
quit()
#############################################################################################################################
plt.savefig('../../docs/' + patient_name + p + '.pdf')
with open(os.path.join(directory, 'uprite_hs_to.pkl'), 'wb') as afile:
pickle.dump(save_data, afile)
print("Completed HS & TO for patient: ", patient_name)
print('Successful run!')
print('-----------RUNTIME: %s second ----' %
(clocktime.time() - start_time))
