def walk_direction_attitude(ax, ay, az, uw, ux, uy, uz, plot_test=False):
"""
Estimate local walk (not cardinal) directions by rotation with attitudes.
Translated by Arno Klein from Elias Chaibub-Neto's R code.
Parameters
----------
ax : list or numpy array
x-axis accelerometer data
ay : list or numpy array
y-axis accelerometer data
az : list or numpy array
z-axis accelerometer data
uw : list or numpy array
w of attitude quaternion
ux : list or numpy array
x of attitude quaternion
uy : list or numpy array
y of attitude quaternion
uz : list or numpy array
z of attitude quaternion
plot_test : Boolean
plot rotated vectors?
Returns
-------
directions : list of lists of three floats
unit vector of local walk (not cardinal) direction at each time point
Examples
--------
>>> from mhealthx.xio import read_accel_json
>>> from mhealthx.signals import compute_sample_rate
>>> input_file = '/Users/arno/DriveWork/mhealthx/mpower_sample_data/deviceMotion_walking_outbound.json.items-a2ab9333-6d63-4676-977a-08591a5d837f5221783798792869048.tmp'
>>> device_motion = True
>>> start = 0
>>> t, axyz, gxyz, wxyz, rxyz, sample_rate, duration = read_accel_json(input_file, device_motion, start)
>>> ax, ay, az = axyz
>>> uw, ux, uy, uz = wxyz
>>> from mhealthx.extractors.pyGait import walk_direction_attitude
>>> plot_test = True
>>> directions = walk_direction_attitude(ax, ay, az, uw, ux, uy, uz, plot_test)
"""
import numpy as np
def quaternion_rotation_matrix(q):
"""Rotation matrix from a quaternion"""
r11 = q[0]**2 + q[1]**2 - q[2]**2 - q[3]**2
r12 = 2 * q[1] * q[2] + 2 * q[0] * q[3]
r13 = 2 * q[1] * q[3] - 2 * q[0] * q[2]
r21 = 2 * q[1] * q[2] - 2 * q[0] * q[3]
r22 = q[0]**2 - q[1]**2 + q[2]**2 - q[3]**2
r23 = 2 * q[2] * q[3] + 2 * q[0] * q[1]
r31 = 2 * q[1] * q[3] + 2 * q[0] * q[2]
r32 = 2 * q[2] * q[3] - 2 * q[0] * q[1]
r33 = q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2
rot = np.array(((r11, r12, r13), (r21, r22, r23), (r31, r32, r33)))
return rot
def rotate_with_attitude(attitude, acceleration):
"""Rotate acceleration with attitude quaternion"""
rot = quaternion_rotation_matrix(attitude)
rotated = np.dot(rot, acceleration)
return rotated
directions = []
for i, x in enumerate(ax):
attitude = [uw[i], ux[i], uy[i], uz[i]]
acceleration = [x, ay[i], az[i]]
directions.append(rotate_with_attitude(attitude, acceleration))
# Plot vectors:
if plot_test:
from mhealthx.utilities import plot_vectors
dx = [x[0] for x in directions]
dy = [x[1] for x in directions]
dz = [x[2] for x in directions]
title = 'Acceleration vectors + attitude-rotated vectors'
plot_vectors(ax, ay, az, dx, dy, dz, title)
return directions
def walk_direction_preheel(ax, ay, az, t, sample_rate,
stride_fraction=1.0/8.0, threshold=0.5,
order=4, cutoff=5, plot_test=False):
"""
Estimate local walk (not cardinal) direction with pre-heel strike phase.
Inspired by Nirupam Roy's B.E. thesis: "WalkCompass:
Finding Walking Direction Leveraging Smartphone's Inertial Sensors,"
this program derives the local walk direction vector from the end
of the primary leg's stride, when it is decelerating in its swing.
While the WalkCompass relies on clear heel strike signals across the
accelerometer axes, this program just uses the most prominent strikes,
and estimates period from the real part of the FFT of the data.
NOTE::
This algorithm computes a single walk direction, and could compute
multiple walk directions prior to detected heel strikes, but does
NOT estimate walking direction for every time point, like
walk_direction_attitude().
Parameters
----------
ax : list or numpy array
x-axis accelerometer data
ay : list or numpy array
y-axis accelerometer data
az : list or numpy array
z-axis accelerometer data
t : list or numpy array
accelerometer time points
sample_rate : float
sample rate of accelerometer reading (Hz)
stride_fraction : float
fraction of stride assumed to be deceleration phase of primary leg
threshold : float
ratio to the maximum value of the summed acceleration across axes
plot_test : Boolean
plot most prominent heel strikes?
Returns
-------
direction : numpy array of three floats
unit vector of local walk (not cardinal) direction
Examples
--------
>>> from mhealthx.xio import read_accel_json
>>> from mhealthx.signals import compute_sample_rate
>>> input_file = '/Users/arno/DriveWork/mhealthx/mpower_sample_data/deviceMotion_walking_outbound.json.items-a2ab9333-6d63-4676-977a-08591a5d837f5221783798792869048.tmp'
>>> device_motion = True
>>> start = 150
>>> t, axyz, gxyz, uxyz, rxyz, sample_rate, duration = read_accel_json(input_file, start, device_motion)
>>> ax, ay, az = axyz
>>> from mhealthx.extractors.pyGait import walk_direction_preheel
>>> threshold = 0.5
>>> stride_fraction = 1.0/8.0
>>> order = 4
>>> cutoff = max([1, sample_rate/10])
>>> plot_test = True
>>> direction = walk_direction_preheel(ax, ay, az, t, sample_rate, stride_fraction, threshold, order, cutoff, plot_test)
"""
import numpy as np
from mhealthx.extractors.pyGait import heel_strikes
from mhealthx.signals import compute_interpeak
# Sum of absolute values across accelerometer axes:
data = np.abs(ax) + np.abs(ay) + np.abs(az)
# Find maximum peaks of smoothed data:
plot_test2 = False
dummy, ipeaks_smooth = heel_strikes(data, sample_rate, threshold,
order, cutoff, plot_test2, t)
# Compute number of samples between peaks using the real part of the FFT:
interpeak = compute_interpeak(data, sample_rate)
decel = np.int(np.round(stride_fraction * interpeak))
# Find maximum peaks close to maximum peaks of smoothed data:
ipeaks = []
for ipeak_smooth in ipeaks_smooth:
ipeak = np.argmax(data[ipeak_smooth - decel:ipeak_smooth + decel])
ipeak += ipeak_smooth - decel
ipeaks.append(ipeak)
# Plot peaks and deceleration phase of stride:
if plot_test:
from pylab import plt
if isinstance(t, list):
tplot = np.asarray(t) - t[0]
else:
tplot = np.linspace(0, np.size(ax), np.size(ax))
idecel = [x - decel for x in ipeaks]
plt.plot(tplot, data, 'k-', tplot[ipeaks], data[ipeaks], 'rs')
for id in idecel:
plt.axvline(x=tplot[id])
plt.title('Maximum stride peaks')
plt.show()
# Compute the average vector for each deceleration phase:
vectors = []
for ipeak in ipeaks:
decel_vectors = np.asarray([[ax[i], ay[i], az[i]]
for i in range(ipeak - decel, ipeak)])
vectors.append(np.mean(decel_vectors, axis=0))
# Compute the average deceleration vector and take the opposite direction:
direction = -1 * np.mean(vectors, axis=0)
# Return the unit vector in this direction:
direction /= np.sqrt(direction.dot(direction))
# Plot vectors:
if plot_test:
from mhealthx.utilities import plot_vectors
dx = [x[0] for x in vectors]
dy = [x[1] for x in vectors]
dz = [x[2] for x in vectors]
hx, hy, hz = direction
title = 'Average deceleration vectors + estimated walk direction'
plot_vectors(dx, dy, dz, [hx], [hy], [hz], title)
return direction