def run_example(typ_filter="martin"):
""" run example : "mahony" or "martin"
"""
# Compute (True) or load (False
[params_acc, params_mag, params_gyr] = calib_param(compute=False)
# Load the recording data
[time_imu, accx, accy, accz, mx, my, mz, gyrx, gyry, gyrz] = \
load_foxcsvfile(DATAFILE)
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()
# Computation loop
for i in range(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
if i == 0:
if typ_filter == "mahony":
quat[0, :] = [1, 0, 0, 0]
else:
quat[0] = observer.init_observer(
np.hstack([acc_imu[0, :], mag_imu[0, :], gyr_imu[0, :]]))
else:
if typ_filter == "mahony":
quat[i] = mahony.update(
quat[i - 1],
np.hstack([acc_imu[i, :], mag_imu[i, :], gyr_imu[i, :]]))
euler[i] = quat2euler(quat[i])
else:
quat[i] = observer.update(
np.hstack([acc_imu[i, :], mag_imu[i, :], gyr_imu[i, :]]),
0.005)
euler[i] = quat2euler(quat[i])
#Plot results
plot_quat(typ_filter, time_imu,\
quat[:,0], quat[:,1], quat[:,2], quat[:,3])
if typ_filter == "mahony":
plot_euler(typ_filter, time_imu,\
euler[:,2], euler[:,1], euler[:,0])
else:
plot_euler(typ_filter, time_imu,\
euler[:,2], euler[:,1], euler[:,0])
def update(self, data):
[self.quaternion] = np.array(madgwick.update(self.quaternion, data))
self.euler = np.array(quat2euler(self.quaternion))
print 'Quaternion : ' + str(self.quaternion) +'\n' + 'Rz : '+'%0.2f' %((self.euler[0])*180/pi)+' '+u'°' +\
' Ry : '+'%0.2f' %((self.euler[1])*180/pi)+' '+u'°' + ' Rx : '+'%0.2f' %((self.euler[2])*180/pi)+' '+u'°'
# Rotation along z
self.rot_z_display.text = 'Z Rotation : ' + '%0.2f' % (
(self.euler[0]) * 180 / pi) + ' ' + u'°'
# Rotation along y
self.rot_y_display.text = 'Y Rotation : ' + '%0.2f' % (
(self.euler[1]) * 180 / pi) + ' ' + u'°'
# Rotation along x
self.rot_x_display.text = 'X Rotation : ' + '%0.2f' % (
(self.euler[2]) * 180 / pi) + ' ' + u'°'
# Put the IMU in its original position
self.IMU.axis = (8, 0, 0)
self.IMU.up = (1, 0, 0)
# Rotate it of arccos(qw)*2 on the axis qx,qy,qz
self.IMU.rotate(origin=(0, 0, 0),
angle=np.arccos(self.quaternion[0]) * 2,
axis=(self.quaternion[1:4]))
def update(self, data):
self.quaternion = nq.mult(self.quat_offset,
self.observer.update(data[0, 2:12], 0.005))
self.euler = np.array(quat2euler(self.quaternion))
print 'Quaternion : ' + str(self.quaternion) +'\n' + 'Rz : '+'%0.2f' %((self.euler[0])*180/pi)+' '+u'°' +\
' Ry : '+'%0.2f' %((self.euler[1])*180/pi)+' '+u'°' + ' Rx : '+'%0.2f' %((self.euler[2])*180/pi)+' '+u'°'
# Rotation along z
self.rot_z_display.text = 'Z Rotation : ' + '%0.2f' % (
(self.euler[0]) * 180 / pi) + ' ' + u'°'
# Rotation along y
self.rot_y_display.text = 'Y Rotation : ' + '%0.2f' % (
(self.euler[1]) * 180 / pi) + ' ' + u'°'
# Rotation along x
self.rot_x_display.text = 'X Rotation : ' + '%0.2f' % (
(self.euler[2]) * 180 / pi) + ' ' + u'°'
# Put the IMU in its original position
self.IMU.axis = (8, 0, 0)
self.IMU.up = (1, 0, 0)
# Rotate it of arccos(qw)*2 on the axis qx,qy,qz
if self.quaternion[0] >= -1 and self.quaternion[0] <= 1:
self.IMU.rotate(origin=(0, 0, 0),
angle=np.arccos(self.quaternion[0]) * 2,
axis=(self.quaternion[1:4]))
def test_quats():
for x, y, z in eg_rots:
M1 = nea.euler2mat(z, y, x)
quatM = nq.mat2quat(M1)
quat = nea.euler2quat(z, y, x)
yield nq.nearly_equivalent, quatM, quat
quatS = sympy_euler2quat(z, y, x)
yield nq.nearly_equivalent, quat, quatS
zp, yp, xp = nea.quat2euler(quat)
# The parameters may not be the same as input, but they give the
# same rotation matrix
M2 = nea.euler2mat(zp, yp, xp)
yield assert_array_almost_equal, M1, M2
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 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 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 goniometer_example():
calib_sensor_shank = 'data/CALIB_SENSORS/1_IMU23_SHANK.csv' #rot
calib_sensor_thigh = 'data/CALIB_SENSORS/1_IMU22_THIGH.csv' #ref
calib_geom_shank = 'data/CALIB_GEOM/2_IMU23_SHANK.csv'
calib_geom_thigh = 'data/CALIB_GEOM/2_IMU22_THIGH.csv'
trial_file_shank = 'data/STAND_TO_SIT_TO_STAND_TO_SIT/3_IMU23_SHANK.csv'
trial_file_thigh = 'data/STAND_TO_SIT_TO_STAND_TO_SIT/3_IMU22_THIGH.csv'
######################################################
# Sensors (scale/offset) calib parameters computation
######################################################
[params_acc_ref, params_mag_ref, params_gyr_ref] = \
calib.compute(imuNumber=22 ,filepath=calib_sensor_thigh, param = 3)
[params_acc_rot, params_mag_rot, params_gyr_rot] = \
calib.compute(imuNumber=23 ,filepath=calib_sensor_shank, param = 3)
scale_acc_ref = params_acc_ref[1:4,:]
bias_acc_ref = params_acc_ref[0,:]
scale_mag_ref = params_mag_ref[1:4,:]
bias_mag_ref = params_mag_ref[0,:]
scale_gyr_ref = params_gyr_ref[1:4,:]
bias_gyr_ref = params_gyr_ref[0,:]
scale_acc_rot = params_acc_rot[1:4,:]
bias_acc_rot = params_acc_rot[0,:]
scale_mag_rot = params_mag_rot[1:4,:]
bias_mag_rot = params_mag_rot[0,:]
scale_gyr_rot = params_gyr_rot[1:4,:]
bias_gyr_rot = params_gyr_rot[0,:]
###########################################
# Geometrical calib (q_offset computation)
###########################################
# load data from the recording calib
[time_imu_calib_ref, accx_calib_ref, accy_calib_ref, accz_calib_ref, mx_calib_ref,\
my_calib_ref, mz_calib_ref, gyrx_calib_ref, gyry_calib_ref, gyrz_calib_ref] = \
load_foxcsvfile(calib_geom_thigh)
[time_imu_calib_rot, accx_calib_rot, accy_calib_rot, accz_calib_rot, mx_calib_rot,\
my_calib_rot, mz_calib_rot, gyrx_calib_rot, gyry_calib_rot, gyrz_calib_rot] = \
load_foxcsvfile(calib_geom_shank)
############################
acc_imu_calib_ref = np.column_stack([accx_calib_ref, accy_calib_ref, accz_calib_ref])
mag_imu_calib_ref = np.column_stack([mx_calib_ref, my_calib_ref, mz_calib_ref])
gyr_imu_calib_ref = np.column_stack([gyrx_calib_ref, gyry_calib_ref, gyrz_calib_ref])
# Applies scale and offset on raw calib trial data (ref)
for i in range(0,len(acc_imu_calib_ref)-1):
acc_imu_calib_ref[i,:]= np.transpose(np.dot(scale_acc_ref,np.transpose((acc_imu_calib_ref[i,:]-np.transpose(bias_acc_ref)))))
mag_imu_calib_ref[i,:]= np.transpose(np.dot(scale_mag_ref,np.transpose((mag_imu_calib_ref[i,:]-np.transpose(bias_mag_ref)))))
gyr_imu_calib_ref[i,:]= np.transpose(np.dot(scale_gyr_ref,np.transpose((gyr_imu_calib_ref[i,:]-np.transpose(bias_gyr_ref)))))
############################
acc_imu_calib_rot = np.column_stack([accx_calib_rot, accy_calib_rot, accz_calib_rot])
mag_imu_calib_rot = np.column_stack([mx_calib_rot, my_calib_rot, mz_calib_rot])
gyr_imu_calib_rot = np.column_stack([gyrx_calib_rot, gyry_calib_rot, gyrz_calib_rot])
# Applies scale and offset on raw calib trial data (rot)
for i in range(0,len(acc_imu_calib_rot)-1):
acc_imu_calib_rot[i,:]= np.transpose(np.dot(scale_acc_rot,np.transpose((acc_imu_calib_rot[i,:]-np.transpose(bias_acc_rot)))))
mag_imu_calib_rot[i,:]= np.transpose(np.dot(scale_mag_rot,np.transpose((mag_imu_calib_rot[i,:]-np.transpose(bias_mag_rot)))))
gyr_imu_calib_rot[i,:]= np.transpose(np.dot(scale_gyr_rot,np.transpose((gyr_imu_calib_rot[i,:]-np.transpose(bias_gyr_rot)))))
############################
data_ref_calib_geom = np.hstack((acc_imu_calib_ref, mag_imu_calib_ref, gyr_imu_calib_ref))
data_rot_calib_geom = np.hstack((acc_imu_calib_rot, mag_imu_calib_rot, gyr_imu_calib_rot))
q_offset = \
calib_geom.compute(data_ref_calib_geom, data_rot_calib_geom)
#######################
# Trial data loading
#######################
# Loads trial data (ref imu thigh)
[time_imu_ref, accx_ref, accy_ref, accz_ref, mx_ref, my_ref, mz_ref, gyrx_ref, gyry_ref, gyrz_ref] = \
load_foxcsvfile(trial_file_thigh)
# Loads trial data (rot imu shank)
[time_imu_rot, accx_rot, accy_rot, accz_rot, mx_rot, my_rot, mz_rot, gyrx_rot, gyry_rot, gyrz_rot] = \
load_foxcsvfile(trial_file_shank)
################################
# q_ref quaternion computation #
################################
# Applies the Scale and Offset to data ref
acc_imu_ref = np.column_stack([accx_ref, accy_ref, accz_ref])
mag_imu_ref = np.column_stack([mx_ref, my_ref, mz_ref])
gyr_imu_ref = np.column_stack([gyrx_ref, gyry_ref, gyrz_ref])
# Quaternions calculation q_ref (thigh)
observer_ref = martin.martin_ahrs()
euler_ref = np.zeros((len(acc_imu_ref),3))
q_ref = np.zeros((len(acc_imu_ref),4))
data_ref_imu0 = np.mean(data_ref_calib_geom[:,0:10],0)
data_ref_imu1 = np.mean(data_rot_calib_geom[:,0:10],0)
#q_ref[0,:] = observer_ref.init_observer(np.hstack([acc_imu_ref[0,:],mag_imu_ref[0,:], gyr_imu_ref[0,:]]))
q_ref[0,:] = observer_ref.init_observer(data_ref_imu0) #build the observer from the mean values of the geom calib position
for i in range(1,len(acc_imu_ref)-1):
# acc_imu_ref[i,:]= np.transpose(np.dot(scale_acc_ref,np.transpose((acc_imu_ref[i,:]-np.transpose(bias_acc_ref)))))
mag_imu_ref[i,:]= np.transpose(np.dot(scale_mag_ref,np.transpose((mag_imu_ref[i,:]-np.transpose(bias_mag_ref)))))
gyr_imu_ref[i,:]= np.transpose(np.dot(scale_gyr_ref,np.transpose((gyr_imu_ref[i,:]-np.transpose(bias_gyr_ref)))))
q_ref[i+1]= observer_ref.update(np.hstack([acc_imu_ref[i,:],mag_imu_ref[i,:], gyr_imu_ref[i,:]]), 0.005)
euler_ref[i]=quat2euler(q_ref[i+1])
################################
# q_rot quaternion computation #
################################
# Applies the Scale and Offset to data rot
acc_imu_rot = np.column_stack([accx_rot, accy_rot, accz_rot])
mag_imu_rot = np.column_stack([mx_rot, my_rot, mz_rot])
gyr_imu_rot = np.column_stack([gyrx_rot, gyry_rot, gyrz_rot])
# Quaternions calculation q_rot (shank)
observer_rot = martin.martin_ahrs()
euler_rot = np.zeros((len(acc_imu_rot),3))
q_rot = np.zeros((len(acc_imu_rot),4))
#q_rot[0,:] = observer_rot.init_observer(np.hstack([acc_imu_rot[0,:],mag_imu_rot[0,:], gyr_imu_rot[0,:]]))
q_rot[0,:] = observer_rot.init_observer(data_ref_imu1) #build the observer from the mean values of the geom calib position
for i in range(0,len(acc_imu_rot)-1):
# acc_imu_rot[i,:]= np.transpose(np.dot(scale_acc_rot,np.transpose((acc_imu_rot[i,:]-np.transpose(bias_acc_rot)))))
mag_imu_rot[i,:]= np.transpose(np.dot(scale_mag_rot,np.transpose((mag_imu_rot[i,:]-np.transpose(bias_mag_rot)))))
gyr_imu_rot[i,:]= np.transpose(np.dot(scale_gyr_rot,np.transpose((gyr_imu_rot[i,:]-np.transpose(bias_gyr_rot)))))
q_rot[i+1]= observer_rot.update(np.hstack([acc_imu_rot[i,:],mag_imu_rot[i,:], gyr_imu_rot[i,:]]), 0.005)
euler_rot[i]= quat2euler(q_rot[i+1])
###########################################################################
# Compute angles from the two quaternions
angle = np.zeros((len(q_ref)-1))
rot_axis = np.zeros((len(q_ref),3))
euler = np.zeros((len(q_ref),3))
for i in range(0,len(q_ref)-1):
[angle[i], rot_axis[i]] = \
goniometer.compute(q_ref[i], q_rot[i], q_offset.reshape(4,))
euler[i]=quat2euler(np.hstack((angle[i],rot_axis[i])))
f, axis = plt.subplots(2, sharex=True)
# Plot angle
axis[0].plot(angle*180/np.pi, label='angle')
axis[0].set_title('Angle')
axis[0].set_ylabel('deg')
axis[0].legend()
# Plot rot axis
axis[1].plot(rot_axis)
axis[1].set_title('Rot. Axis')
axis[1].legend(('x','y','z'))