def test_markley():
# Compute
[params_acc, params_mag, params_gyr] = \
calib.compute(imuNumber=4 ,filepath="data/3D_validation/CALIB_ACCEL_MAGNETO/4_IMU4_WAND.csv")
# Load the recording data
[time_imu, accx, accy, accz, mx, my, mz, _, _, _] = \
load_foxcsvfile("data/3D_validation/3/3_IMU4_WAND.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, :]
acc_imu = np.column_stack([accx, accy, accz])
mag_imu = np.column_stack([mx, my, mz])
quaternion = np.zeros((len(acc_imu), 4))
for i in range(0, len(acc_imu)):
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)))))
quaternion[i] = markley.compute(
np.hstack([acc_imu[i, :], mag_imu[i, :]]))
return quaternion
def param(datacalibfile, calibfile, comments=""):
""" Load or compute calibration parameters
"""
[params_acc, params_mag, params_gyr] = \
calib.compute(imuNumber=5, filepath=datacalibfile, param=3)
calib.save_param(calibfile, params_acc, params_mag, params_gyr, comments)
return [params_acc, params_mag, params_gyr]
def calib_param(compute=False):
""" Load or compute calibration parameters
"""
if compute == True:
[params_acc, params_mag, params_gyr] = \
calib.compute(imuNumber=5 ,filepath=DATACALIBFILE, param = 3)
else:
[params_acc, params_mag, params_gyr] = \
calib.load_param(CALIBFILE)
return [params_acc, params_mag, params_gyr]
def calib_param(compute=True):
""" Load or compute calibration parameters
"""
if compute == True:
[params_acc, params_mag, params_gyr] = \
calib.compute(imuNumber=5 ,filepath=DATACALIBFILE, param = 3)
calib.save_param(CALIBFILE,
params_acc,
params_mag,
params_gyr,
comments="Expe Prima")
else:
[params_acc, params_mag, params_gyr] = \
calib.load_param(CALIBFILE)
return [params_acc, params_mag, params_gyr]
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 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'))