def init_observer(self, z):
""" Martin Salaun init observer
"""
# Quaternions construction
ya = [0, z[0], z[1], z[2]]
yb = [0, z[3], z[4], z[5]]
yc = nq.mult(ya, yb)
# Normalization
ya = nq.normalize(ya)
# yb = nq.normalize(yb)
yc[0] = 0
yc = nq.normalize(yc)
if ya[3] == 1:
self.q = 1
self.qinv = 1
else:
self.qinv = [-ya[2], 1 - ya[3], 0, ya[1]]
self.qinv = nq.normalize(self.qinv)
self.q = nq.conjugate(self.qinv)
yc = nq.mult(nq.mult(self.q, yc), self.qinv)
if yc[2] != 1:
self.qinv = nq.mult(self.qinv, [-yc[1], 0, yc[3], 1 - yc[2]])
self.qinv = nq.normalize(self.qinv)
self.q = nq.conjugate(self.qinv)
return self.q
def compute(q_0, q_1, q_offset=[1, 0, 0, 0]):
""" Computes rotation angle between two quaternions
Parameters :
------------
q_0 : numpy array of float
the first quaternion (w, x, y, z)
q_1 : numpy array of float
the second quaternion (w, x, y, z)
q_offset : numpy array of float
the initial quaternion corresponding to the offset
between q_0 and q_1 at an initial position
(can be generated using sensbiotk.calib.calib_geom)
Returns
-------
angle : float
the angle of rotation between q_0 and q_1 (rad)
rot_axis : numpy array of float
the axis of rotation
"""
q_corr = nq.mult(
nq.mult(np.transpose(nq.conjugate(q_0)), np.transpose(q_1)),
nq.conjugate(q_offset)) # q_corr = conj(q_0) x q_1 x conj(q_offset)
angle = np.arccos(q_corr[0]) * 2 # angle = acos(q_corr(w)) x 2
rot_axis = q_corr[1:4] # the axis of rotation between
# the two quaternions (ex: if along Z, should be close to [0, 0, 1])
return angle, rot_axis
def compute(data_imu0, data_imu1):
""" Compute an "offset" quaternion between two supposed aligned quaternions
Parameters :
------------
data_imu0 : numpy array of float
the IMU column stacked reference position data [ACC MAG GYR]
data_imu1 : numpy array of float
the IMU column stacked reference position data [ACC MAG GYR]
Returns
-------
q_offset : quaternion [w, x, y, z]
the offset quaternion between IMU0 and IMU1
"""
# Averages the data on the reference position
data_ref_imu0 = np.mean(data_imu0[:, 0:10], 0)
data_ref_imu1 = np.mean(data_imu1[:, 0:10], 0)
# madgwick calculation
# q_init=[1, 0, 0, 0]
# q0 = madgwick.update(q_init, data_ref_imu0)
# q1 = madgwick.update(q_init, data_ref_imu1)
# Uses martin salaun for calculating the two quaternions
init0 = martin.martin_ahrs()
init1 = martin.martin_ahrs()
q0 = init0.init_observer(data_ref_imu0)
q1 = init1.init_observer(data_ref_imu1)
q_offset = nq.mult(np.transpose(nq.conjugate(q0)),
np.transpose(q1)) # q_offset = conj(q0) x q1
return q_offset
def update(q, z, fs=200):
accelero = z[0:3]
magneto = z[3:6]
gyro = z[6:9]
sample_period = 1. / fs
Kp = 0.01 # algorithm proportional gain
Ki = 0 # algorithm integral gain
# Normalise accelerometer measurement
norm_accelero = norm(accelero)
if norm_accelero != 0:
accelero = accelero / norm_accelero
# Normalise magnetometer measurement
norm_magneto = norm(magneto)
if norm_magneto != 0:
magneto = magneto / norm_magneto
# Reference direction of Earth's magnetic field
h = nq.mult(q, nq.mult(np.concatenate(([0], magneto)), nq.conjugate(q)))
b = np.array([0, np.linalg.norm([h[1], h[2]]), 0, h[3]])
# Estimated direction of gravity and magnetic field
v = np.array([[2 * (q[1] * q[3] - q[0] * q[2])],
[2 * (q[0] * q[1] + q[2] * q[3])],
[q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2]])
w = np.array([[
2 * b[1] * (0.5 - q[2]**2 - q[3]**2) + 2 * b[3] *
(q[1] * q[3] - q[0] * q[2])
],
[
2 * b[1] * (q[1] * q[2] - q[0] * q[3]) + 2 * b[3] *
(q[0] * q[1] + q[2] * q[3])
],
[
2 * b[1] * (q[0] * q[2] + q[1] * q[3]) + 2 * b[3] *
(0.5 - q[1]**2 - q[2]**2)
]])
# Error is sum of cross product between
# estimated direction and measured direction of fields
e = np.cross(np.transpose(accelero), np.transpose(v)) + np.cross(
np.transpose(magneto), np.transpose(w))
if (Ki > 0):
obj.eInt = obj.eInt + e * sample_period
# Apply feedback terms
gyro = np.transpose(gyro + Kp * e + Ki * obj.eInt)
# Compute rate of change of quaternion
q_dot = np.dot(0.5, nq.mult(q, (0, gyro[0], gyro[1], gyro[2])))
# Integrate to yield quaternion
q = q + np.transpose((q_dot * sample_period))
return q / np.linalg.norm(q)
def run():
""" run example
"""
# Load data
[_, _, mag_calib, _] = load_data(data_calib_file)
# Compute calib params
[offset_mag, scale_mag] = calib_mag_param(mag_calib)
params_mag = np.vstack((offset_mag, scale_mag))
# Load data
[time_imu, acc_imu, mag_imu, gyr_imu] = load_data(data_file)
# Init output
quat = np.zeros((len(acc_imu), 4))
quat_corr = np.zeros((len(acc_imu), 4))
euler = np.zeros((len(acc_imu), 3))
observer = martin_ahrs.martin_ahrs()
data_init = np.mean(
np.hstack([acc_imu[0:100, :], mag_imu[0:100, :], gyr_imu[0:100, :]]),
0) # mean of the first static 2 seconds
quat[0] = observer.init_observer(data_init)
# Computation loop
for i in range(0, len(acc_imu)):
# Applies the Scale and Offset to data
mag_imu[i, :] = normalize_data(mag_imu[i, :], params_mag)
# Filter call
quat[i] = observer.update(
np.hstack([acc_imu[i, :], mag_imu[i, :], gyr_imu[i, :]]), 0.005)
conj_q_init = nq.conjugate(quat[100, :])
for i in range(0, len(acc_imu) - 1):
quat_corr[i + 1] = nq.mult(conj_q_init, quat[i + 1])
euler[i] = quat2euler(quat_corr[i + 1])
#load vicon data
head_angle_vicon = np.loadtxt('data/3/head_angle_vicon.txt')
time_delay = 0.3
#Plot results
plt.hold(True)
plt.plot(np.arange(0,
len(head_angle_vicon) / 100., 0.01), head_angle_vicon)
plt.plot(time_imu - time_delay,
euler[:, 0] * 180 / np.pi - 6,
'k--',
linewidth='1.5')
plt.legend(('head_angle_vicon', 'head_angle_imu'))
plt.xlabel('Time (s)')
plt.ylabel('Angle (deg)')
def test_martin_ahrs():
quat_dict = sio.loadmat('data/test_martin_ahrs/quaternion_martin.mat')
quat_verif = quat_dict['quaternion']
[_, params_mag, params_gyr] = \
calib.load_param("data/test_martin_ahrs/CalibrationFileIMU6.txt")
# Load the recording data
[_, accx, accy, accz, mx, my, mz, gyrx, gyry, gyrz] = \
load_foxcsvfile("data/test_martin_ahrs/1_IMU6_RIGHT_FOOT.csv")
# Applies the Scale and Offset to data
scale_mag = params_mag[1:4, :]
bias_mag = params_mag[0, :]
scale_gyr = params_gyr[1:4, :]
bias_gyr = params_gyr[0, :]
acc_imu = np.column_stack([accx, accy, accz])
mag_imu = np.column_stack([mx, my, mz])
gyr_imu = np.column_stack([gyrx, gyry, gyrz])
observer = martin.martin_ahrs()
quaternion = np.zeros((len(acc_imu), 4))
quaternion_corr = np.zeros((len(acc_imu), 4))
data_init = np.mean(np.hstack(
[acc_imu[0:200, :], \
mag_imu[0:200, :], \
gyr_imu[0:200, :]]), 0) # mean of the first static 2 seconds
quaternion[0, :] = observer.init_observer(data_init)
for i in range(1, len(acc_imu)-1):
mag_imu[i, :] = np.transpose(np.dot(
scale_mag, np.transpose((mag_imu[i, :]-np.transpose(bias_mag)))))
gyr_imu[i, :] = np.transpose(np.dot(
scale_gyr, np.transpose((gyr_imu[i, :]-np.transpose(bias_gyr)))))
quaternion[i+1] = observer.update(
np.hstack([acc_imu[i, :], mag_imu[i, :], gyr_imu[i, :]]), 0.005)
conj_q_init = nq.conjugate(quaternion[150, :])
for i in range(1, len(acc_imu)-1):
quaternion_corr[i+1] = nq.mult(conj_q_init, quaternion[i+1])
yield assert_equal, quaternion_corr.all() == quat_verif.all(), "OK"
def compute_values(self):
while self.run:
try:
quat = euler.euler2quat(self.heading*np.pi/180,self.attitude*np.pi/180,self.bank*np.pi/180)
if self.quat_offset == [0, 0, 0]:
self.quat_offset = quat
print('======')
print(str(self.quat_offset))
else:
quat = q.mult(q.conjugate(self.quat_offset),quat)
self.rad_pedal_angle = euler.quat2euler2(quat)[0]
self.pedal_angle = (-self.rad_pedal_angle*180/np.pi)
if self.pedal_angle < 0 :
self.pedal_angle = self.pedal_angle + 360
self.plot()
time.sleep(0.010)
#save computed values in file
if self.rec == True:
self.data_out.write('%.1f \t %.1f \t %.1f \t %.1f \t %.1f \t %.1f \t %.1f \t %.1f \n' %(self.time_clock_imu1 - self.offset_time, self.pedal_angle, self.bike_speed, self.distance, ((self.gyr_z*60)/(2*np.pi)), self.hr_ptap.power, self.hr_ptap.total_power, self.hr_ptap.hr))
except:
continue
def update(self, z, sample_period):
""" Martin Salaun iteration observer
"""
# Quaternions construction
ya = np.array([0, z[0], z[1], z[2]])
# ya = nq.normalize(ya)
yb = np.array([0, z[3], z[4], z[5]])
# yb = nq.normalize(yb)
wm = np.array([0, z[6], z[7], z[8]])
# wm = nq.normalize(wm)
# Compute quaternions products
yc = nq.mult(ya, yb)
yc[0] = 0
yd = nq.mult(yc, ya)
yd[0] = 0
# Compute errors
EA = self._A - \
nq.mult(self.q, nq.mult(ya, self.qinv)) / self.a_s
EC = self._C - \
nq.mult(self.q, nq.mult(yc, self.qinv)) / self.c_s
ED = self._D - \
nq.mult(self.q, nq.mult(yd, self.qinv)) / (self.a_s * self.c_s)
# Numerical stabilisation
EA[0] = 0
EC[0] = 0
ED[0] = 0
# sEA = <EA, EA - A> = ||EA||² - <EA, A>
sEA = nq.norm(EA) - EA[3]
# sEC = <EC, EC - C> = ||EC||² - <EC, C>
sEC = nq.norm(EC) - EC[2]
# sED = <ED, ED - D> = ||ED||² - <ED, D>
sED = nq.norm(ED) - ED[1]
LE = nq.mult(self._A, EA) * self.la + nq.mult(self._C, EC) * self.lc \
+ nq.mult(self._D, ED) * self.ld
LE[0] = 0
ME = LE * (-self.sigma)
if self.la + self.ld != 0:
NE = self.n / (self.la + self.ld) * (self.la * sEA + self.ld * sED)
else:
NE = 0
if self.lc + self.ld != 0:
OE = self.o / (self.lc + self.ld) * (self.lc * sEC + self.ld * sED)
else:
OE = 0
qdot = nq.mult(self.q, wm - self.wb) * 0.5 + \
nq.mult(LE, self.q) + self.q * (self.k * (1 - nq.norm(self.q)))
wbdot = nq.mult(nq.mult(self.qinv, ME), self.q)
a_sdot = self.a_s * NE
csdot = self.c_s * OE
# Integration
self.q = self.q + qdot * sample_period
self.wb = self.wb + wbdot * sample_period
self.a_s = self.a_s + a_sdot * sample_period
self.c_s = self.c_s + csdot * sample_period
# compute inverse for the next step
self.qinv = nq.conjugate(self.q)
qrot = nq.mult([0, 1, 0, 0], self.q)
return qrot
def tug():
trial_number = 8
calib_sensor_shank_right = 'data/CALIB/9_IMU4_RIGHT_SHANK.csv' #rot
calib_sensor_thigh_right = 'data/CALIB/9_IMU6_RIGHT_THIGH.csv' #ref
calib_sensor_shank_left = 'data/CALIB/9_IMU5_LEFT_SHANK.csv' #rot
calib_sensor_thigh_left = 'data/CALIB/9_IMU9_LEFT_THIGH.csv' #ref
calib_sensor_trunk = 'data/CALIB/9_IMU8_TRUNK.csv'
trial_file_shank_right = 'data/' + str(trial_number) + '/' + str(
trial_number) + '_IMU4_RIGHT_SHANK.csv'
trial_file_thigh_right = 'data/' + str(trial_number) + '/' + str(
trial_number) + '_IMU6_RIGHT_THIGH.csv'
trial_file_shank_left = 'data/' + str(trial_number) + '/' + str(
trial_number) + '_IMU5_LEFT_SHANK.csv'
trial_file_thigh_left = 'data/' + str(trial_number) + '/' + str(
trial_number) + '_IMU9_LEFT_THIGH.csv'
trial_file_trunk = 'data/' + str(trial_number) + '/' + str(
trial_number) + '_IMU8_TRUNK.csv'
######################################################
# Sensors (scale/offset) calib parameters computation
######################################################
[_, params_mag_right_thigh, _] = \
calib.compute(imuNumber=6 ,filepath=calib_sensor_thigh_right, param = 3)
[_, params_mag_right_shank, _] = \
calib.compute(imuNumber=4 ,filepath=calib_sensor_shank_right, param = 3)
[_, params_mag_left_thigh, _] = \
calib.compute(imuNumber=9 ,filepath=calib_sensor_thigh_left, param = 3)
[_, params_mag_left_shank, _] = \
calib.compute(imuNumber=5 ,filepath=calib_sensor_shank_left, param = 3)
[_, params_mag_trunk, _] = \
calib.compute(imuNumber=8 ,filepath=calib_sensor_trunk, param = 3)
scale_mag_right_thigh = params_mag_right_thigh[1:4, :]
bias_mag_right_thigh = params_mag_right_thigh[0, :]
scale_mag_right_shank = params_mag_right_shank[1:4, :]
bias_mag_right_shank = params_mag_right_shank[0, :]
scale_mag_left_thigh = params_mag_left_thigh[1:4, :]
bias_mag_left_thigh = params_mag_left_thigh[0, :]
scale_mag_left_shank = params_mag_left_shank[1:4, :]
bias_mag_left_shank = params_mag_left_shank[0, :]
scale_mag_trunk = params_mag_left_shank[1:4, :]
bias_mag_trunk = params_mag_left_shank[0, :]
######################################################
# Load trials data
######################################################
# right thigh
[time_imu_right_thigh, accx_right_thigh, accy_right_thigh, accz_right_thigh, \
mx_right_thigh, my_right_thigh, mz_right_thigh, gyrx_right_thigh, gyry_right_thigh, gyrz_right_thigh] = \
load_foxcsvfile(trial_file_thigh_right)
# left thigh
[time_imu_left_thigh, accx_left_thigh, accy_left_thigh, accz_left_thigh, \
mx_left_thigh, my_left_thigh, mz_left_thigh, gyrx_left_thigh, gyry_left_thigh, gyrz_left_thigh] = \
load_foxcsvfile(trial_file_thigh_left)
# right shank
[time_imu_right_shank, accx_right_shank, accy_right_shank, accz_right_shank, \
mx_right_shank, my_right_shank, mz_right_shank, gyrx_right_shank, gyry_right_shank, gyrz_right_shank] = \
load_foxcsvfile(trial_file_shank_right)
# left shank
[time_imu_left_shank, accx_left_shank, accy_left_shank, accz_left_shank, \
mx_left_shank, my_left_shank, mz_left_shank, gyrx_left_shank, gyry_left_shank, gyrz_left_shank] = \
load_foxcsvfile(trial_file_shank_left)
# trunk
[time_imu_trunk, accx_trunk, accy_trunk, accz_trunk, \
mx_trunk, my_trunk, mz_trunk, gyrx_trunk, gyry_trunk, gyrz_trunk] = \
load_foxcsvfile(trial_file_trunk)
######################################################
# Applies scale and offset on magnetometer data
######################################################
nb_common_samples = np.amin([len(time_imu_right_thigh), len(time_imu_left_thigh), len(time_imu_right_shank),\
len(time_imu_left_shank), len(time_imu_trunk)])
acc_imu_right_thigh = np.column_stack([
accx_right_thigh[0:nb_common_samples],
accy_right_thigh[0:nb_common_samples],
accz_right_thigh[0:nb_common_samples]
])
mag_imu_right_thigh = np.column_stack([
mx_right_thigh[0:nb_common_samples],
my_right_thigh[0:nb_common_samples],
mz_right_thigh[0:nb_common_samples]
])
gyr_imu_right_thigh = np.column_stack([
gyrx_right_thigh[0:nb_common_samples],
gyry_right_thigh[0:nb_common_samples],
gyrz_right_thigh[0:nb_common_samples]
])
acc_imu_left_thigh = np.column_stack([
accx_left_thigh[0:nb_common_samples],
accy_left_thigh[0:nb_common_samples],
accz_left_thigh[0:nb_common_samples]
])
mag_imu_left_thigh = np.column_stack([
mx_left_thigh[0:nb_common_samples], my_left_thigh[0:nb_common_samples],
mz_left_thigh[0:nb_common_samples]
])
gyr_imu_left_thigh = np.column_stack([
gyrx_left_thigh[0:nb_common_samples],
gyry_left_thigh[0:nb_common_samples],
gyrz_left_thigh[0:nb_common_samples]
])
acc_imu_right_shank = np.column_stack([
accx_right_shank[0:nb_common_samples],
accy_right_shank[0:nb_common_samples],
accz_right_shank[0:nb_common_samples]
])
mag_imu_right_shank = np.column_stack([
mx_right_shank[0:nb_common_samples],
my_right_shank[0:nb_common_samples],
mz_right_shank[0:nb_common_samples]
])
gyr_imu_right_shank = np.column_stack([
gyrx_right_shank[0:nb_common_samples],
gyry_right_shank[0:nb_common_samples],
gyrz_right_shank[0:nb_common_samples]
])
acc_imu_left_shank = np.column_stack([
accx_left_shank[0:nb_common_samples],
accy_left_shank[0:nb_common_samples],
accz_left_shank[0:nb_common_samples]
])
mag_imu_left_shank = np.column_stack([
mx_left_shank[0:nb_common_samples], my_left_shank[0:nb_common_samples],
mz_left_shank[0:nb_common_samples]
])
gyr_imu_left_shank = np.column_stack([
gyrx_left_shank[0:nb_common_samples],
gyry_left_shank[0:nb_common_samples],
gyrz_left_shank[0:nb_common_samples]
])
acc_imu_trunk = np.column_stack([
accx_trunk[0:nb_common_samples], accy_trunk[0:nb_common_samples],
accz_trunk[0:nb_common_samples]
])
mag_imu_trunk = np.column_stack([
mx_trunk[0:nb_common_samples], my_trunk[0:nb_common_samples],
mz_trunk[0:nb_common_samples]
])
gyr_imu_trunk = np.column_stack([
gyrx_trunk[0:nb_common_samples], gyry_trunk[0:nb_common_samples],
gyrz_trunk[0:nb_common_samples]
])
# Applies scale and offset
for i in range(0, nb_common_samples):
mag_imu_right_thigh[i, :] = np.transpose(
np.dot(
scale_mag_right_thigh,
np.transpose((mag_imu_right_thigh[i, :] -
np.transpose(bias_mag_right_thigh)))))
mag_imu_left_thigh[i, :] = np.transpose(
np.dot(
scale_mag_left_thigh,
np.transpose((mag_imu_left_thigh[i, :] -
np.transpose(bias_mag_left_thigh)))))
mag_imu_right_shank[i, :] = np.transpose(
np.dot(
scale_mag_right_shank,
np.transpose((mag_imu_right_shank[i, :] -
np.transpose(bias_mag_right_shank)))))
mag_imu_left_shank[i, :] = np.transpose(
np.dot(
scale_mag_left_shank,
np.transpose((mag_imu_left_shank[i, :] -
np.transpose(bias_mag_left_shank)))))
mag_imu_trunk[i, :] = np.transpose(
np.dot(
scale_mag_trunk,
np.transpose(
(mag_imu_trunk[i, :] - np.transpose(bias_mag_trunk)))))
###########################################
# Geometrical calib (q_offset computation)
###########################################
data_static_right_thigh = np.hstack(
(acc_imu_right_thigh[0:1200], mag_imu_right_thigh[0:1200],
gyr_imu_right_thigh[0:1200]))
data_static_left_thigh = np.hstack(
(acc_imu_left_thigh[0:1200], mag_imu_left_thigh[0:1200],
gyr_imu_left_thigh[0:1200]))
data_static_right_shank = np.hstack(
(acc_imu_right_shank[0:1200], mag_imu_right_shank[0:1200],
gyr_imu_right_shank[0:1200]))
data_static_left_shank = np.hstack(
(acc_imu_left_shank[0:1200], mag_imu_left_shank[0:1200],
gyr_imu_left_shank[0:1200]))
data_static_trunk = np.hstack(
(acc_imu_trunk[0:1200], mag_imu_trunk[0:1200], gyr_imu_trunk[0:1200]))
q_offset_right = \
calib_geom.compute(data_static_right_thigh, data_static_right_shank)
q_offset_left = \
calib_geom.compute(data_static_left_thigh, data_static_left_shank)
################################
# Quaternions computation #
################################
# Observers initiations
observer_right_thigh = martin.martin_ahrs()
observer_left_thigh = martin.martin_ahrs()
observer_right_shank = martin.martin_ahrs()
observer_left_shank = martin.martin_ahrs()
observer_trunk = martin.martin_ahrs()
# euler_ref = np.zeros((len(acc_imu_ref),3))
# Quaternions initiation
q_right_thigh = np.zeros((nb_common_samples, 4))
q_right_shank = np.zeros((nb_common_samples, 4))
q_left_thigh = np.zeros((nb_common_samples, 4))
q_left_shank = np.zeros((nb_common_samples, 4))
q_trunk = np.zeros((nb_common_samples, 4))
# Init data
data_init_right_thigh = np.mean(data_static_right_thigh[:, 0:10], 0)
data_init_left_thigh = np.mean(data_static_left_thigh[:, 0:10], 0)
data_init_right_shank = np.mean(data_static_right_shank[:, 0:10], 0)
data_init_left_shank = np.mean(data_static_left_shank[:, 0:10], 0)
data_init_trunk = np.mean(data_static_trunk[:, 0:10], 0)
q_right_thigh[0, :] = observer_right_thigh.init_observer(
data_init_right_thigh
) #build the observer from the mean values of the geom calib position
q_left_thigh[0, :] = observer_left_thigh.init_observer(
data_init_left_thigh
) #build the observer from the mean values of the geom calib position
q_right_shank[0, :] = observer_right_shank.init_observer(
data_init_right_shank
) #build the observer from the mean values of the geom calib position
q_left_shank[0, :] = observer_left_shank.init_observer(
data_init_left_shank
) #build the observer from the mean values of the geom calib position
q_trunk[0, :] = observer_trunk.init_observer(
data_init_trunk
) #build the observer from the mean values of the geom calib position
for i in range(0, nb_common_samples - 1):
q_right_thigh[i + 1] = observer_right_thigh.update(
np.hstack([
acc_imu_right_thigh[i, :], mag_imu_right_thigh[i, :],
gyr_imu_right_thigh[i, :]
]), 0.005)
q_left_thigh[i + 1] = observer_left_thigh.update(
np.hstack([
acc_imu_left_thigh[i, :], mag_imu_left_thigh[i, :],
gyr_imu_left_thigh[i, :]
]), 0.005)
q_right_shank[i + 1] = observer_right_shank.update(
np.hstack([
acc_imu_right_shank[i, :], mag_imu_right_shank[i, :],
gyr_imu_right_shank[i, :]
]), 0.005)
q_left_shank[i + 1] = observer_left_shank.update(
np.hstack([
acc_imu_left_shank[i, :], mag_imu_left_shank[i, :],
gyr_imu_left_shank[i, :]
]), 0.005)
q_trunk[i + 1] = observer_trunk.update(
np.hstack([
acc_imu_trunk[i, :], mag_imu_trunk[i, :], gyr_imu_trunk[i, :]
]), 0.005)
###########################################################################
# Compute angles from the quaternions
###########################################################################
knee_angle_right = np.zeros((nb_common_samples, 1))
knee_angle_left = np.zeros((nb_common_samples, 1))
for i in range(0, nb_common_samples - 1):
[knee_angle_left[i], _] = \
goniometer.compute(q_left_thigh[i], q_left_shank[i], q_offset_left.reshape(4,))
[knee_angle_right[i], _] = \
goniometer.compute(q_right_thigh[i], q_right_shank[i], q_offset_right.reshape(4,))
# Frame change from NEDown to initial frame
conj_q_init_trunk = nq.conjugate(q_trunk[1200, :])
euler_trunk = np.zeros((nb_common_samples, 3))
for i in range(0, nb_common_samples - 1):
q_trunk[i] = nq.mult(conj_q_init_trunk, q_trunk[i])
euler_trunk[i] = quat2euler(q_trunk[i])
# Plots
# plt.figure()
# plt.hold(True)
# plt.plot(euler_trunk[:,2]*180/pi)
# plt.plot(knee_angle_right*180/pi)
# plt.plot(knee_angle_left*180/pi)
# Save the calculated quaternions to a .mat file
quat_dict = {}
quat_dict['knee_angle_right'] = knee_angle_right
quat_dict['knee_angle_left'] = knee_angle_left
quat_dict['quaternion_trunk'] = q_trunk
quat_dict['euler_trunk_ZYX'] = euler_trunk
sio.savemat('export_' + str(trial_number) + '.mat', quat_dict)
def run_example():
""" run example : "martin"
"""
# Compute (True) or load (False
[params_acc, params_mag, params_gyr] = calib_param(compute=False)
# Load the recording data
[time_sens, accx, accy, accz, mx, my, mz, gyrx, gyry, gyrz] = \
load_foxcsvfile(DATAFILE)
# Find motionless begin periods
freqs = 200
start, end = find_static_periods(gyrz, 2 * np.pi / 180, 3 * freqs)
static_duration = time_sens[end[0]] - time_sens[start[0]]
print "LGHT", start[0], len(end)
if static_duration < 5.0:
print "Warning: static duration too low"
time_imu = time_sens
acc_imu = np.column_stack([accx, accy, accz])
mag_imu = np.column_stack([mx, my, mz])
gyr_imu = np.column_stack([gyrx, gyry, gyrz])
# Init output
quat = np.zeros((len(acc_imu), 4))
euler = np.zeros((len(acc_imu), 3))
observer = martin_ahrs.martin_ahrs()
quat_offset = [1, 0, 0, 0]
# Initialization loop
for i in range(0, end[0]):
# Applies the Scale and Offset to data
acc_imu[i, :] = normalize_data(acc_imu[i, :], params_acc)
mag_imu[i, :] = normalize_data(mag_imu[i, :], params_mag)
gyr_imu[i, :] = normalize_data(gyr_imu[i, :], params_gyr)
# Filter call
if i == 0:
quat[0] = observer.init_observer(
np.hstack([acc_imu[0, :], mag_imu[0, :], gyr_imu[0, :]]))
else:
quat[i] = observer.update(
np.hstack([acc_imu[i, :], mag_imu[i, :], gyr_imu[i, :]]),
0.005)
quat_offset = nq.conjugate(quat[end - 1][0])
print "Quaternion init", quat_offset
# Computation loop
for i in range(end[0], len(acc_imu)):
# Applies the Scale and Offset to data
acc_imu[i, :] = normalize_data(acc_imu[i, :], params_acc)
mag_imu[i, :] = normalize_data(mag_imu[i, :], params_mag)
gyr_imu[i, :] = normalize_data(gyr_imu[i, :], params_gyr)
# Filter call
quat[i] = observer.update(
np.hstack([acc_imu[i, :], mag_imu[i, :], gyr_imu[i, :]]), 0.005)
quat[i] = nq.mult(quat_offset, quat[i])
euler[i] = quat2euler(quat[i])
# Plot results
plot_quat("Expe Prima ", time_imu,\
quat[:,0], quat[:,1], quat[:,2], quat[:,3])
plot_euler("Expe Prima ", time_imu,\
euler[:,2], euler[:,1], euler[:,0])
# Save results
save_ahrs_csvfile(ANGLEFILE, time_imu, quat, euler)
def update(q, z, fs=200):
"""
Updates the computed q quaternion
"""
accelero = z[0:3]
magneto = z[3:6]
gyro = z[6:9]
sample_period = 1. / fs
Beta = 0.02
# Normalise accelerometer measurement
norm_accelero = norm(accelero)
if norm_accelero != 0:
accelero = accelero / norm_accelero
# Normalise magnetometer measurement
norm_magneto = norm(magneto)
if norm_magneto != 0:
magneto = magneto / norm_magneto
# Reference direction of Earth's magnetic field
h = nq.mult(q, nq.mult(np.concatenate(([0], magneto)), nq.conjugate(q)))
# h = [1, 1, 1, 1]
b = np.array([0, np.linalg.norm([h[1], h[2]]), 0, h[3]])
# Gradient decent algorithm corrective step
F = np.array([[2 * (q[1] * q[3] - q[0] * q[2]) - accelero[0]],
[2 * (q[0] * q[1] + q[2] * q[3]) - accelero[1]],
[2 * (0.5 - q[1]**2 - q[2]**2) - accelero[2]],
[
2 * b[1] * (0.5 - q[2]**2 - q[3]**2) + 2 * b[3] *
(q[1] * q[3] - q[0] * q[2]) - magneto[0]
],
[
2 * b[1] * (q[1] * q[2] - q[0] * q[3]) + 2 * b[3] *
(q[0] * q[1] + q[2] * q[3]) - magneto[1]
],
[
2 * b[1] * (q[0] * q[2] + q[1] * q[3]) + 2 * b[3] *
(0.5 - q[1]**2 - q[2]**2) - magneto[2]
]])
J = np.array([[-2 * q[2], 2 * q[3], -2 * q[0], 2 * q[1]],
[2 * q[1], 2 * q[0], 2 * q[3], 2 * q[2]],
[0, -4 * q[1], -4 * q[2], 0],
[
-2 * b[3] * q[2], 2 * b[3] * q[3],
4 * b[1] * q[2] - 2 * b[3] * q[0],
-4 * b[1] * q[3] + 2 * b[3] * q[1]
],
[
-2 * b[1] * q[3] + 2 * b[3] * q[1],
2 * b[1] * q[2] + 2 * b[3] * q[0],
2 * b[1] * q[1] + 2 * b[3] * q[3],
-2 * b[1] * q[0] + 2 * b[3] * q[2]
],
[
2 * b[1] * q[2], 2 * b[1] * q[3] - 4 * b[3] * q[1],
2 * b[1] * q[0] - 4 * b[3] * q[2], 2 * b[1] * q[1]
]])
step = (np.dot(np.transpose(J), F))
# normalise step magnitude
step = step / norm(step)
# Compute rate of change of quaternion
q_dot = np.dot(
0.5, nq.mult(q, (0, gyro[0], gyro[1], gyro[2]))) - \
np.dot(Beta, np.transpose(step))
# Integrate to yield quaternion
q = q + (q_dot * sample_period)
return q / np.linalg.norm(q)
def VPython_quaternion_3D():
# Compute
# [params_acc, params_mag, params_gyr] = \
# calib.compute(imuNumber=6 ,filepath="data/CALIB.csv", param = 3)
[params_acc, params_mag, params_gyr] = \
calib.load_param("data/CalibrationFileIMU6.txt")
# Load the recording data
[time_imu, accx, accy, accz, mx, my, mz, gyrx, gyry, gyrz] = \
load_foxcsvfile("data/1_IMU6_RIGHT_FOOT.csv")
# Applies the Scale and Offset to data
scale_acc = params_acc[1:4, :]
bias_acc = params_acc[0, :]
scale_mag = params_mag[1:4, :]
bias_mag = params_mag[0, :]
scale_gyr = params_gyr[1:4, :]
bias_gyr = params_gyr[0, :]
acc_imu = np.column_stack([accx, accy, accz])
mag_imu = np.column_stack([mx, my, mz])
gyr_imu = np.column_stack([gyrx, gyry, gyrz])
quaternion = np.zeros((len(acc_imu), 4))
euler = np.zeros((len(acc_imu), 3))
quaternion[0, :] = [1, 0, 0, 0]
for i in range(0, len(acc_imu) - 1):
acc_imu[i, :] = np.transpose(
np.dot(scale_acc,
np.transpose((acc_imu[i, :] - np.transpose(bias_acc)))))
mag_imu[i, :] = np.transpose(
np.dot(scale_mag,
np.transpose((mag_imu[i, :] - np.transpose(bias_mag)))))
gyr_imu[i, :] = np.transpose(
np.dot(scale_gyr,
np.transpose((gyr_imu[i, :] - np.transpose(bias_gyr)))))
quaternion[i + 1] = madgwick.update(
quaternion[i],
np.hstack([acc_imu[i, :], mag_imu[i, :], gyr_imu[i, :]]))
euler[i] = quat2euler(quaternion[i + 1])
#######################
# Plots
#######################
dataVicon = np.loadtxt('data/RIGHT_FOOT.txt', skiprows=4)
time_vicon = dataVicon[:, 0]
euler_vicon = np.zeros((len(time_vicon), 3))
quat_offset = np.hstack(
[dataVicon[0, 7], dataVicon[0, 4], dataVicon[0, 5], dataVicon[0, 6]])
for i in range(1, len(time_vicon) - 1):
quat_vicon = np.hstack([
dataVicon[i, 7], dataVicon[i, 4], dataVicon[i, 5], dataVicon[i, 6]
])
euler_vicon[i] = quat2euler(
nq.mult(nq.conjugate(quat_offset), quat_vicon))
#Z
plt.figure()
plt.hold(True)
plt.plot(time_imu - (0.2), euler[:, 0] * 180 / pi)
plt.plot(time_vicon, euler_vicon[:, 0] * 180 / pi)
plt.legend(('z_imu', 'z_vicon'))
xlabel('time (s)')
ylabel('angle (deg)')
#Y
plt.figure()
plt.hold(True)
plt.plot(time_imu - (0.2), euler[:, 1] * 180 / pi)
plt.plot(time_vicon, euler_vicon[:, 1] * 180 / pi)
plt.legend(('y_imu', 'y_vicon'))
xlabel('time (s)')
ylabel('angle (deg)')
plt.show()
#X
plt.figure()
plt.hold(True)
plt.plot(time_imu - (0.2), euler[:, 2] * 180 / pi)
plt.plot(time_vicon, euler_vicon[:, 2] * 180 / pi)
plt.legend(('x_imu', 'x_vicon'))
xlabel('time (s)')
ylabel('angle (deg)')
plt.show()
quat_dict = {}
quat_dict['quaternion'] = quaternion
sio.savemat('quaternion_madgwick.mat', quat_dict)
def compute_attitude(datafile, calibfile, anglefile, plotting=True):
""" compute attitude IMU sensor
"""
# Load calibration parameters
[params_acc, params_mag, params_gyr] = calib.load_param(calibfile)
# Load the recording data
[time_sens, accx, accy, accz, magx, magy, magz, gyrx, gyry, gyrz] = \
load_foxcsvfile(datafile)
# Find motionless begin periods
freqs = 200
start, end = find_static_periods(gyrz, 2 * np.pi / 180, 3 * freqs)
static_duration = time_sens[end[0]] - time_sens[start[0]]
if static_duration < 5.0:
print "Warning: static duration too low"
time_imu = time_sens
acc_imu = np.column_stack([accx, accy, accz])
mag_imu = np.column_stack([magx, magy, magz])
gyr_imu = np.column_stack([gyrx, gyry, gyrz])
# Init output
quat = np.zeros((len(acc_imu), 4))
euler = np.zeros((len(acc_imu), 3))
observer = martin_ahrs.martin_ahrs()
# Initialization loop
quat_offset = [1, 0, 0, 0]
for i in range(0, end[0]):
# Applies the Scale and Offset to data
acc_imu[i, :] = normalize_data(acc_imu[i, :], params_acc)
mag_imu[i, :] = normalize_data(mag_imu[i, :], params_mag)
gyr_imu[i, :] = normalize_data(gyr_imu[i, :], params_gyr)
# Filter call
if i == 0:
quat[0] = observer.init_observer(
np.hstack([acc_imu[0, :], mag_imu[0, :], gyr_imu[0, :]]))
else:
quat[i] = observer.update(
np.hstack([acc_imu[i, :], mag_imu[i, :], gyr_imu[i, :]]),
0.005)
quat_offset = nq.conjugate(quat[end - 1][0])
print "Quaternion init", quat_offset
# Computation loop
for i in range(end[0], len(acc_imu)):
# Applies the Scale and Offset to data
acc_imu[i, :] = normalize_data(acc_imu[i, :], params_acc)
mag_imu[i, :] = normalize_data(mag_imu[i, :], params_mag)
gyr_imu[i, :] = normalize_data(gyr_imu[i, :], params_gyr)
# Filter call
quat[i] = observer.update(
np.hstack([acc_imu[i, :], mag_imu[i, :], gyr_imu[i, :]]), 0.005)
quat[i] = nq.mult(quat_offset, quat[i])
euler[i] = quat2euler(quat[i])
# Plot results
if plotting is True:
plot_quat("Expe Prima ", time_imu, quat[:, 0], quat[:, 1], quat[:, 2],
quat[:, 3])
plot_euler("Expe Prima ", time_imu, euler[:, 2], euler[:, 1], euler[:,
0])
# Save results
save_ahrs_csvfile(anglefile, time_imu, quat, euler)
def VPython_quaternion_3D():
# Compute
## Example 1
## [params_acc, params_mag, params_gyr] = \
## calib.compute(imuNumber=6 ,filepath="data/CALIB.csv", param = 3)
#
# [params_acc, params_mag, params_gyr] = \
# calib.load_param("data/CalibrationFileIMU6.txt")
# # Load the recording data
# [time_imu, accx, accy, accz, mx, my, mz, gyrx, gyry, gyrz] = \
# load_foxcsvfile("data/1_IMU6_RIGHT_FOOT.csv")
# dataVicon = np.loadtxt('data/RIGHT_FOOT.txt',skiprows=4)
# Example 2
[params_acc, params_mag, params_gyr] = \
calib.compute(imuNumber=5 ,filepath="data2/CALIB.csv", param = 3)
#
# [params_acc, params_mag, params_gyr] = \
# calib.load_param("data2/CalibrationFileIMU5.txt")
# Load the recording data
[time_imu, accx, accy, accz, mx, my, mz, gyrx, gyry, gyrz] = \
load_foxcsvfile("data2/5_IMU5_LEFT_FOOT.csv")
dataVicon = np.loadtxt('data2/LEFT_FOOT.txt', skiprows=4)
time_delay = 3.05
## Example 3
# [params_acc, params_mag, params_gyr] = \
# calib.compute(imuNumber=5 ,filepath="data3/CALIB.csv", param = 3)
###
## [params_acc, params_mag, params_gyr] = \
## calib.load_param("data2/CalibrationFileIMU5.txt")
##
## # Load the recording data
# [time_imu, accx, accy, accz, mx, my, mz, gyrx, gyry, gyrz] = \
# load_foxcsvfile("data3/4_IMU5_LEFT_FOOT.csv")
##
# dataVicon = np.loadtxt('data3/LEFT_FOOT.txt',skiprows=4)
# time_delay = 0.35
# Applies the Scale and Offset to data
scale_mag = params_mag[1:4, :]
bias_mag = params_mag[0, :]
scale_gyr = params_gyr[1:4, :]
bias_gyr = params_gyr[0, :]
acc_imu = np.column_stack([accx, accy, accz])
mag_imu = np.column_stack([mx, my, mz])
gyr_imu = np.column_stack([gyrx, gyry, gyrz])
observer = martin.martin_ahrs()
quaternion = np.zeros((len(acc_imu), 4))
quat_imu_corr = np.zeros((len(acc_imu), 4))
euler = np.zeros((len(acc_imu), 3))
data_init = np.mean(
np.hstack([acc_imu[0:200, :], mag_imu[0:200, :], gyr_imu[0:200, :]]),
0) # mean of the first static 2 seconds
quaternion[0, :] = observer.init_observer(data_init)
for i in range(1, len(acc_imu) - 1):
mag_imu[i, :] = np.transpose(
np.dot(scale_mag,
np.transpose((mag_imu[i, :] - np.transpose(bias_mag)))))
gyr_imu[i, :] = np.transpose(
np.dot(scale_gyr,
np.transpose((gyr_imu[i, :] - np.transpose(bias_gyr)))))
quaternion[i + 1] = observer.update(
np.hstack([acc_imu[i, :], mag_imu[i, :], gyr_imu[i, :]]), 0.005)
conj_q_init = nq.conjugate(quaternion[150, :])
for i in range(1, len(acc_imu) - 1):
quat_imu_corr[i + 1] = nq.mult(conj_q_init, quaternion[i + 1])
euler[i] = quat2euler(quat_imu_corr[i + 1])
#######################
# Plots
#######################
time_vicon = dataVicon[:, 0]
euler_vicon = np.zeros((len(time_vicon), 3))
quat_vicon_corr = np.zeros((len(time_vicon), 4))
quat_offset = np.hstack(
[dataVicon[0, 7], dataVicon[0, 4], dataVicon[0, 5], dataVicon[0, 6]])
for i in range(1, len(time_vicon) - 1):
quat_vicon = np.hstack([
dataVicon[i, 7], dataVicon[i, 4], dataVicon[i, 5], dataVicon[i, 6]
])
quat_vicon_corr[i] = nq.mult(nq.conjugate(quat_offset), quat_vicon)
euler_vicon[i] = quat2euler(quat_vicon_corr[i])
#Z
plt.figure()
plt.hold(True)
plt.plot(time_imu - time_delay, euler[:, 0] * 180 / pi)
plt.plot(time_vicon, euler_vicon[:, 0] * 180 / pi)
plt.legend(('z_imu', 'z_vicon'))
xlabel('time (s)')
ylabel('angle (deg)')
#Y
plt.figure()
plt.hold(True)
plt.plot(time_imu - time_delay, euler[:, 1] * 180 / pi)
plt.plot(time_vicon, euler_vicon[:, 1] * 180 / pi)
plt.legend(('y_imu', 'y_vicon'))
xlabel('time (s)')
ylabel('angle (deg)')
plt.show()
#X
plt.figure()
plt.hold(True)
plt.plot(time_imu - time_delay, euler[:, 2] * 180 / pi)
plt.plot(time_vicon, euler_vicon[:, 2] * 180 / pi)
plt.legend(('x_imu', 'x_vicon'))
xlabel('time (s)')
ylabel('angle (deg)')
plt.show()
# Save the calculated quaternions to a .mat file
quat_dict = {}
quat_dict['quat_imu_corr'] = quat_imu_corr
quat_dict['euler_imu_corr_ZYX'] = euler
quat_dict['quat_vicon_corr'] = quat_vicon_corr
quat_dict['euler_vicon_corr_ZYX'] = euler_vicon
quat_dict['time_imu_corr'] = time_imu - time_delay
quat_dict['time_vicon_corr'] = time_vicon
sio.savemat('export.mat', quat_dict)
def real_time_martin():
rt = RT_Martin()
# instanciate dongle
foxdongle = fdongle.FoxDongle()
# init 3d visu
rt.f_3D_plot_init()
print 'Enter acquisition loop (type ^C to stop).'
init = False
init_observer = False
init_frame = False
calib_mag_bool = False
scale = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
offset = np.array([0, 0, 0])
while True:
try:
# handle dongle initialization
if not init and foxdongle.init_dongle():
init = True
print 'Device is connected to %s.' % (foxdongle.line())
if foxdongle.is_running():
data = foxdongle.read()
if data is not None and data.shape == (11, ):
data = data.reshape(1, 11)
data[0, 5:8] = np.transpose(
np.dot(
scale,
np.transpose(
(data[0, 5:8] - np.transpose(offset)))))
if not calib_mag_bool: # record 40s of data for the magnetometer calibration
print 'Calib magneto, 20 seconds \n'
data_calib = np.zeros((4000, 11))
for i in range(1, 4000):
data_calib[i] = foxdongle.read().reshape(1, 11)
time.sleep(0.005)
offset, scale = calib_mag.compute(data_calib[:, 5:8])
print scale
print offset
print 'Calib magneto over'
print 'Initializing the observer...'
calib_mag_bool = True
if not init_observer and calib_mag_bool: # record data for initializing the observer
print 'Please stay without moving in the initial position (as VPython)'
time.sleep(4)
data_obs = np.zeros((1000, 11))
for i in range(0, 1000):
data = foxdongle.read().reshape(1, 11)
data[0, 5:8] = np.transpose(
np.dot(
scale,
np.transpose((data[0, 5:8] -
np.transpose(offset)))))
data_obs[i, :] = data[:]
time.sleep(0.005)
print('.'),
rt.init_observer(np.mean(data_obs[:, 2:12], 0))
init_observer = True
print 'Start animation...'
rt.text.height = 0
if init_observer and not init_frame:
for i in range(
0, 1000): # waits that the algorithm converges
data = foxdongle.read().reshape(1, 11)
data[0, 5:8] = np.transpose(
np.dot(
scale,
np.transpose((data[0, 5:8] -
np.transpose(offset)))))
rt.update(data)
time.sleep(0.005)
rt.quat_offset = nq.conjugate(rt.quaternion)
init_frame = True
else:
rt.update(data)
time.sleep(0.005)
except KeyboardInterrupt:
print "\nStopped"
break
except Exception as e:
logging.error('exception reached:' + str(e))
# must close to kill read thread (fox_sink)
foxdongle.close_dongle()
print 'Done'
def test_conjugate():
# Takes sequence
cq = nq.conjugate((1, 0, 0, 0))
# Returns float type
yield assert_true, cq.dtype.kind == 'f'