import numpy as np import matplotlib.pyplot as plt from scipy.signal import argrelmax import sys import math import argparse def rotate_data_fhws(data, data_t, rotation, rotation_t): #Invert rotationmatrix np.linalg.inv(rotation) #Align rotation time according to data time tmp = [] for t in data_t: # Find indices of roation matrix that are earlier #than the current time of the sensor value ind = np.where(rotation_t <= t)[0] #Use the last index if len(ind) != 0: tmp.append(ind[-1]) else: tmp.append(0) #Only use the values of the rotation matrix that are aligned with the sensor data rotation = rotation[tmp] # Multiply data with rotationmatrix rot_data = [] for i, row in enumerate(data): row = np.append(row, 1) rot_data.append(np.dot(rotation[i], row)) return np.array(rot_data) def rotate_data_lukas(data, rotation): #Invert rotationmatrix np.linalg.inv(rotation) rot_data = [] for i, row in enumerate(data): row = np.append(row, 1) rot_data.append(np.dot(rotation[i], row)) return np.array(rot_data) def magnitude(x, y, z): ret = [math.sqrt(i) for i in (x**2 + y**2 + z**2)] mean = np.mean(ret) ret -= mean return ret def count_steps(time, signal, lt, ht, dead): """ Find steps in the accelerometer signal. After a step was found, a "dead" period exists, where no step can be found again. This is to avoid too many steps Parameters ---------- time: array_like Timestaps of accelerometer signal Must have same length as signal signal: array_like Accelerometer signal of all three axis. Must have same length as time lt: float Low threshold, which must be exceeded by the accelerometer signal to be counted as step ht: float High treshold, which must not be exceeded by the accelerometer signal to be counted as step dead: float After a step was detected, during the dead time no other step will be found. Given in milliseconds """ time_signal = zip(time, signal) dead_time = 0 steps = [] for i in time_signal: if lt < i[1] < ht and i[0] > dead_time: steps.append(i[0]) dead_time = i[0] + dead return np.array(steps) def write_steps_to_file(fname, steps): f = open(fname, 'w') print steps for s in steps: f.write(str(s) + "\n") f.close() def plot_steps(time, signal, steps): plt.title("Step detection") plt.xlabel("ms") plt.ylabel("Accelerometer magnitude") plt.plot(time, signal, label="Accelerometer") s = [] for i,t in enumerate(time): if t in steps: s.append((t, signal[i])) s = np.array(s) plt.plot(s[:,0], s[:,1], 'ro', label = "Steps") plt.legend(numpoints=1) plt.show() def read_data(fname): time = np.loadtxt(fname, delimiter=";", usecols=[0], unpack=True) f = open(fname, 'r') accls = [] accls_t = [] rotations = [] rotations_t = [] start = time[0] for line in f: line = line.split(";") t = int(line[0]) - start #Lin Accel if line[1] == "2": accls_t.append(t) accls.append((line[2], line[3], line[4])) #Rotation elif line[1] == "7": rotations_t.append(t) rotations.append((line[2], line[3], line[4], line[5], line[6], line[7], line[8], line[9], line[10], line[11], line[12],line[13], line[14], line[15], line[16], line[17])) accls = np.array(accls, dtype=float) accls_t = np.array(accls_t, dtype=int) rotations = np.array(rotations, dtype=float) rotations = [row.reshape((4,4)) for row in rotations] rotations = np.array(rotations) rotations_t = np.array(rotations_t, dtype=int) return accls, accls_t, rotations, rotations_t def main(): parser = argparse.ArgumentParser() parser.add_argument("fname_sensor", help = "Accelerometer file") parser.add_argument("fname_output", help = "Output file, where timestamps of steps will be saved") parser.add_argument("--dead", help = "Time span (in ms) after a detected step in which no additional step will be detected (default=600)", type=int) parser.add_argument("--lt", help = "Low threshold, which must be exceeded by the accelerometer signal to be counted as step (default=1.5)", type=float) parser.add_argument("--ht", help = "High treshold, which must not be exceeded by the accelerometer signal to be counted as step(default=6.5)", type=float) parser.add_argument("--plot", help = "Plot step detection", action="store_true") parser.add_argument("--file_format", help = "Sensor data file format [fhws|lukas] (default: fhws)", type = str) args = parser.parse_args() file_format = "fhws" if args.file_format: file_format = args.file_format #My own data format if file_format == "lukas": delimiter = ',' time_cols = [40] accel_cols = [6,7,8] time = np.loadtxt(args.fname_sensor, delimiter=delimiter, usecols=time_cols, skiprows=2, unpack=True) accelX, accelY, accelZ = np.loadtxt(args.fname_sensor, delimiter=delimiter, usecols=accel_cols, skiprows=2, unpack=True) rotation = np.loadtxt(args.fname_sensor, delimiter = delimiter, usecols=range(18,34), skiprows=1, unpack=True) rotations = rotation.T rotations = [row.reshape((4,4)) for row in rotations] accl = np.array([accelX, accelY, accelZ]).T world_accl = rotate_data_lukas(accl, rotations) #FHWS file format else: accls, time, rotation, rotation_t = read_data(args.fname_sensor) world_accl = rotate_data_fhws(accls, time, rotation, rotation_t) accelX = world_accl[:,0] accelY = world_accl[:,1] accelZ = world_accl[:,2] accel_mag = magnitude(accelX, accelY, accelZ) lt = 1.5 ht = 6.5 dead = 600 if args.dead: dead = args.dead if args.lt: lt = args.lt if args.ht: ht = args.ht steps = count_steps(time, accel_mag, lt, ht, dead) print("#Steps detected: ", len(steps)) write_steps_to_file(args.fname_output, steps) if args.plot: plot_steps(time, accel_mag, steps) if __name__ == "__main__": main()