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 test_calib_mag():
""" Test calib_mag function
"""
from sensbiotk.io.iofox import load_foxcsvfile
import sensbiotk.calib.calib_mag as calib_mag
# Load the recording
[_, _, _, _, m_x, m_y, m_z, _, _, _, ] =\
load_foxcsvfile("data/calib_accelerometer/IMU4/HKB0_02.csv")
data = np.column_stack((m_x, m_y, m_z))
offset, scale = calib_mag.compute(data)
# Applies the offset and scale to the data
for i in range(0, len(data)):
data[i, :] = np.transpose(
np.dot(scale, np.transpose((data[i, :] - np.transpose(offset)))))
# Computes the norm on 3D signals.
norm_mag_after_calib = np.mean(
np.sqrt(data[:, 0]**2 + data[:, 1]**2 + data[:, 2]**2))
# It has to be close to 1.0 if the computed parameters are satisfying.
if norm_mag_after_calib < 1.2 and norm_mag_after_calib > 0.8:
resp = True
else:
resp = False
yield assert_equal, resp, True
def test_calib_mag():
""" Test calib_mag function
"""
from sensbiotk.io.iofox import load_foxcsvfile
import sensbiotk.calib.calib_mag as calib_mag
# Load the recording
[_, _, _, _, m_x, m_y, m_z, _, _, _, ] =\
load_foxcsvfile(CALIB_FILE)
data = np.column_stack((m_x, m_y, m_z))
offset, scale = calib_mag.compute(data)
print "OFFSET", offset
print "SCALE", scale
# Applies the offset and scale to the data
for i in range(0, len(data)):
data[i, :] = np.transpose(
np.dot(scale,
np.transpose((data[i, :] - np.transpose(offset)))))
# Plot signals before values calibration
plot_calib(m_x, m_y, m_z, "Raw mag.")
# Plot signals after calibration
norm_mag_after_calib = plot_calib(data[:, 0],
data[:, 1], data[:, 2], "Calib mag.")
# It has to be close to 1.0 if the computed parameters are satisfying.
if norm_mag_after_calib < 1.2 and norm_mag_after_calib > 0.8:
print "CALIB OK"
else:
print "CALIB KO"
return
def example_static_period():
""" Example: static period
"""
from sensbiotk.io.uigetfile import uigetfile
from sensbiotk.io.iofox import load_foxcsvfile
import matplotlib.pyplot as plt
# Load the recording with motionless periods
# For example choose :
# sensbiotk/tests/data/calib_accelerometer/IMU4/HKB0_02.csv
filename =\
uigetfile(
title='Select the IMU .csv file',
filetypes=[('.csv file', '.csv')])
[time, _, _, _, _, _, _, _, _, gyrz] = \
load_foxcsvfile(filename)
# Load the recording with motionless periods
freqs = 200
start, end = algo.find_static_periods(gyrz, 2 * np.pi/180, 3*freqs)
plt.hold(True)
plt.plot(time, gyrz)
for i in range(0, len(start)):
plt.axvline(x=time[start][i], color='g')
plt.axvline(x=time[end][i], color='r')
plt.show()
return
def test_integration_gyro_3D():
[time_imu, _, _, _, _, _, _, gyrx, gyry, gyrz] = \
load_foxcsvfile("data/3D_validation/3/3_IMU4_WAND.csv")
freqs = 200
data_vicon = np.loadtxt("data/3D_validation/2/GLOBAL_EULER_XYZ.txt",
skiprows=2)
time_vicon = data_vicon[:, 0]
theta_x_vicon = data_vicon[:, 1] * 180 / np.pi + 177
theta_y_vicon = data_vicon[:, 2] * 180 / np.pi
theta_z_vicon = data_vicon[:, 3] * 180 / np.pi
#Remove wrong values from Vicon data
theta_x_vicon[np.abs(theta_x_vicon) > 130] = 0
theta_y_vicon[np.abs(theta_y_vicon) > 130] = 0
theta_z_vicon[np.abs(theta_z_vicon) > 130] = 0
# theta_x_vicon[3130:3630] = theta_x_vicon[3129]
# theta_y_vicon[3130:3630] = theta_y_vicon[3129]
# theta_z_vicon[3130:3630] = theta_z_vicon[3129]
plot(time_vicon, theta_x_vicon)
plot(time_vicon, theta_y_vicon)
plot(time_vicon, theta_z_vicon)
theta_x_imu = integration_gyro.compute(gyrx)
theta_y_imu = integration_gyro.compute(gyry)
theta_z_imu = integration_gyro.compute(gyrz)
plot(time_imu, theta_x_imu)
plot(
time_imu,
theta_y_imu,
)
plot(time_imu, theta_z_imu)
legend(('x_vicon', 'y_vicon', 'z_vicon', 'x_imu', 'y_imu', 'z_imu'))
def load_data(data_file):
''' Load the recording data
'''
[time_imu, accx, accy, accz, mx, my, mz, gyrx, gyry, gyrz] = \
load_foxcsvfile(data_file)
acc_imu = np.column_stack([accx, accy, accz])
mag_imu = np.column_stack([mx, my, mz])
gyr_imu = np.column_stack([gyrx, gyry, gyrz])
return [time_imu, acc_imu, mag_imu, gyr_imu]
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 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 test_mahony_ahrs():
quat_dict = sio.loadmat('data/test_mahony_ahrs/quaternion_mahony.mat')
quat_verif = quat_dict['quaternion']
[params_acc, params_mag, params_gyr] = \
calib.load_param("data/test_mahony_ahrs/CalibrationFileIMU6.txt")
# Load the recording data
[_, accx, accy, accz, mx, my, mz, gyrx, gyry, gyrz] = \
load_foxcsvfile("data/test_mahony_ahrs/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))
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] = mahony.update(
quaternion[i],
np.hstack([acc_imu[i, :], mag_imu[i, :], gyr_imu[i, :]]))
yield assert_equal, quaternion.all() == quat_verif.all(), "OK"
def test_static_period():
""" Test: static period
"""
from sensbiotk.io.uigetfile import uigetfile
from sensbiotk.io.iofox import load_foxcsvfile
# Load the recording with motionless periods
[time, _, _, _, _, _, _, _, _, gyrz] = \
load_foxcsvfile("data/calib_accelerometer/IMU4/HKB0_02.csv")
freqs = 200
resp = True
start, end = algo.find_static_periods(gyrz, 2 * np.pi / 180, 3 * freqs)
for i in range(0, len(start)):
# The standard deviation of the gyrz signal must be small
std_static = np.std(gyrz[range(start[i], end[i])])
if std_static > 0.02:
resp = False
yield assert_equal, resp, True
def test_calib_acc():
""" Test calib_acc function
"""
from sensbiotk.algorithms.basic import find_static_periods_3D
from sensbiotk.io.iofox import load_foxcsvfile
import sensbiotk.calib.calib_acc as calib_acc
# Load the recording with motionless periods
[_, accx, accy, accz, _, _, _, gyrx, gyry, gyrz] = \
load_foxcsvfile("data/calib_accelerometer/IMU4/HKB0_02.csv")
freqs = 200
# Detects the motionless periods
start, end = find_static_periods_3D(np.column_stack((gyrx, gyry, gyrz)),
4 * np.pi / 180, 2 * freqs)
acc_stat_x = []
acc_stat_y = []
acc_stat_z = []
for i in range(0, len(start)):
acc_stat_x = np.concatenate(
(np.array(acc_stat_x), accx[start[i]:end[i] + 1]))
acc_stat_y = np.concatenate(
(np.array(acc_stat_y), accy[start[i]:end[i] + 1]))
acc_stat_z = np.concatenate(
(np.array(acc_stat_z), accz[start[i]:end[i] + 1]))
acc_stat = np.column_stack((acc_stat_x, acc_stat_y, acc_stat_z))
# Compute the offset and scale
offset, scale = calib_acc.compute(acc_stat)
acc_imu = np.transpose(
np.vstack(
(np.transpose(accx), np.transpose(accy), np.transpose(accz))))
# if the IMU is well calibrated, the norm's acceleration is equal
# to 9.81 along a motionless period
#g_before_calib = np.sqrt(np.mean(acc_imu[range(start[0], end[0]), 0])**2
# + np.mean(acc_imu[range(start[0], end[0]), 1])**2
# + np.mean(acc_imu[range(start[0], end[0]), 2])**2)
# Applies the offset and scale to the data
for i in range(0, len(acc_imu)):
acc_imu[i, :] = np.transpose(
np.dot(scale, np.transpose(
(acc_imu[i, :] - np.transpose(offset)))))
# Computes the norm on the motionless periods.
# It has to be close to 9.81 if the computed parameters are satisfying.
g_after_calib = np.sqrt(
np.mean(acc_imu[range(start[0], end[0]), 0])**2 +
np.mean(acc_imu[range(start[0], end[0]), 1])**2 +
np.mean(acc_imu[range(start[0], end[0]), 2])**2)
print g_after_calib
if g_after_calib < 9.91 and g_after_calib > 9.71:
resp = True
else:
resp = False
yield assert_equal, resp, True
def compass():
# Compute
# [_, params_mag, _] = \
# calib.compute(imuNumber=5 ,filepath="data/1_IMU5_REGLE.csv", param = 3)
[_, params_mag, _] = \
calib.load_param("data/CalibrationFileIMU5.txt")
# Load the recording data
[time_imu, _, _, _, mx, my, mz, _, _, _] = \
load_foxcsvfile("data/2_IMU5_REGLE.csv")
# Applies the Scale and Offset to data
scale_mag = params_mag[1:4, :]
bias_mag = params_mag[0, :]
mag_imu = np.column_stack([mx, my, mz])
#######################
# 3D objects init
#######################
scene.title = "VISU COMPASS"
scene.autocenter = False
scene.scale = (0.05, 0.05, 0.05)
scene.background = (255, 255, 255)
scene.width = 900
scene.height = 600
scene.forward = (0, 0, -1)
scene.up = (1, 0, 0)
north_frame = arrow(pos=(0, 0, 0),
axis=(10, 0, 0),
shaftwidth=1,
color=color.blue)
west_frame = arrow(pos=(0, 0, 0),
axis=(0, 10, 0),
shaftwidth=1,
color=color.green)
est_frame = arrow(pos=(0, 0, 0),
axis=(0, -10, 0),
shaftwidth=1,
color=color.magenta)
south_frame = arrow(pos=(0, 0, 0),
axis=(-10, 0, 0),
shaftwidth=1,
color=color.yellow)
ring(pos=(0, 0, 0),
axis=(0, 0, -1),
radius=10,
thickness=0.6,
color=color.red)
IMU = box(pos=(0, 0, 0),
axis=(8, 0, 0),
height=5,
width=3,
color=color.black)
angle_display = label(text='Rotation / North : 0 ' + u'°',
yoffset=0,
xoffset=0,
line=False,
box=False,
color=color.blue,
height=18,
pos=(12, 0, 0))
#######################
# 3D Animation
#######################
angle_north = np.zeros((len(mag_imu) - 1, 1))
for i in range(0, len(mag_imu) - 1):
mag_imu[i, :] = np.transpose(
np.dot(scale_mag,
np.transpose((mag_imu[i, :] - np.transpose(bias_mag)))))
angle_north[i] = np.arctan(mag_imu[i, 1] / mag_imu[i, 0]) + (
0.76 * pi / 180) # declination = 0.76° in Montpellier
rot = np.diff(angle_north, axis=0)
rot_cum = np.cumsum(rot)
for k in range(0, len(rot)):
rate(200)
IMU.rotate(origin=(0, 0, 0), angle=rot[k], axis=(0, 0, 1))
angle_display.text = 'Rotation / North : ' + '%0.2f' % (
(rot_cum[k]) * 180 / pi) + ' ' + u'°'
#######################
# Plots
#######################
figure
hold(True)
plot(rot_cum * 180 / pi)
legend(('Rot/North(°)'))
from sensbiotk.io import iofox as fox
import pylab as py
#[time, acc, mag, gyr] = fox.load_foxcsvfile("tmp_imu.csv")
[time, ax, ay, az, mx, my, mz, gx, gy, gz] = \
fox.load_foxcsvfile("tmp_imu.csv")
py.figure()
py.title("Fox logging")
ax1 = py.subplot(311)
py.plot(time, ax)
py.plot(time, ay)
py.plot(time, az)
py.ylabel('ACC (m/s^2)')
py.subplot(312)
py.plot(time, mx)
py.plot(time, my)
py.plot(time, mz)
py.ylabel('MAG (gauss)')
py.subplot(313)
py.plot(time, gx)
py.plot(time, gy)
py.plot(time, gz)
py.ylabel('GYR (rad/s)')
py.xlabel('time (s)')
py.show()
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 compute(imuNumber, filepath=None, param=1):
""" Performs IMU calibration parameters,
saves them in a file at the same path than the .csv recording used,
and returns them.
Parameters :
------------
params : integer
imuNumber, the ID of the IMU to calibrate
filepath
string, the path to the .csv calibration file,
if no path given, a GUI shows up
param
integer, 1 : only returns parameters
2 : only saves parameters
3 : saves and returns parameters
Returns
-------
None
"""
# MAIN PROCESS for loading a calibration recording,
#computing the parameters and saving them in a file.
from sensbiotk.io.uigetfile import uigetfile
from sensbiotk.algorithms.basic import find_static_periods_3D
from sensbiotk.io.iofox import load_foxcsvfile
from os.path import split
# Select and import IMU DATA
if filepath is None:
filepath = uigetfile(title= \
'Select the IMU .csv file', filetypes=[('.csv file', '.csv')])
[_, accx, accy, accz, mx, my, mz, gyrx, gyry, gyrz] = \
load_foxcsvfile(filepath)
freqs = 200
# Detects the motionless periods (at least 2s and less than 4deg/s)
start, end = \
find_static_periods_3D(
np.column_stack((gyrx, gyry, gyrz)), 10 * np.pi/180, 2*freqs)
if len(start) == 0: # if no static periods found
print "WARNING : NO STATIC PERIOD FOUND"
# Computes the parameters
acc_stat_x = []
acc_stat_y = []
acc_stat_z = []
gyr_stat_x = []
gyr_stat_y = []
gyr_stat_z = []
for i in range(0, len(start)):
# Mean of each stationary state
acc_stat_x.append(np.mean(accx[start[i]:end[i] + 1]))
acc_stat_y.append(np.mean(accy[start[i]:end[i] + 1]))
acc_stat_z.append(np.mean(accz[start[i]:end[i] + 1]))
# All the gyro values for each stationary state
gyr_stat_x = np.concatenate(
(np.array(gyr_stat_x), gyrx[start[i]:end[i]]))
gyr_stat_y = np.concatenate(
(np.array(gyr_stat_y), gyry[start[i]:end[i]]))
gyr_stat_z = np.concatenate(
(np.array(gyr_stat_z), gyrz[start[i]:end[i]]))
acc_stat = np.column_stack((acc_stat_x, acc_stat_y, acc_stat_z))
mag = np.column_stack((mx, my, mz))
gyr_stat = np.column_stack((gyr_stat_x, gyr_stat_y, gyr_stat_z))
[params_acc, params_mag, params_gyr] = \
calib_imu_parameters(acc_stat, mag, gyr_stat)
if param == 1: #only returns parameters
return params_acc, params_mag, params_gyr
elif param == 2: #save parameters
save_param(split(filepath)[0]+\
"/CalibrationFileIMU"+str(imuNumber)+".txt",\
params_acc, params_mag, params_gyr, comments="")
elif param == 3: #save and return parameters
save_param(split(filepath)[0]+\
"/CalibrationFileIMU"+str(imuNumber)+".txt",\
params_acc, params_mag, params_gyr, comments="")
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 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 test_calib_acc():
""" Test calib_acc function
"""
from sensbiotk.algorithms.basic import find_static_periods
from sensbiotk.io.iofox import load_foxcsvfile
import sensbiotk.calib.calib_acc as calib_acc
# Load the recording with motionless periods
[_, accx, accy, accz, _, _, _, _, _, gyrz] = \
load_foxcsvfile(CALIB_FILE)
freqs = 200
# Detects the motionless periods
start, end = find_static_periods(gyrz, 2 * np.pi/180, 3*freqs)
acc_stat_x = []
acc_stat_y = []
acc_stat_z = []
for i in range(0, len(start)):
acc_stat_x = np.concatenate((np.array(acc_stat_x),
accx[range(start[i], end[i])]))
acc_stat_y = np.concatenate((np.array(acc_stat_y),
accy[range(start[i], end[i])]))
acc_stat_z = np.concatenate((np.array(acc_stat_z),
accz[range(start[i], end[i])]))
acc_stat = np.column_stack((acc_stat_x, acc_stat_y, acc_stat_z))
# Compute the offset and scale
offset, scale = calib_acc.compute(acc_stat)
print "OFFSET", offset
print "SCALE", scale
acc_imu = np.transpose(np.vstack((np.transpose(accx),
np.transpose(accy), np.transpose(accz))))
# if the IMU is well calibrated, the norm's acceleration is equal
# to 9.81 along a motionless period
#g_before_calib = np.sqrt(np.mean(acc_imu[range(start[0], end[0]), 0])**2
# + np.mean(acc_imu[range(start[0], end[0]), 1])**2
# + np.mean(acc_imu[range(start[0], end[0]), 2])**2)
# Applies the offset and scale to the data
for i in range(0, len(acc_imu)):
acc_imu[i, :] = np.transpose(
np.dot(scale,
np.transpose((acc_imu[i, :] - np.transpose(offset)))))
# Computes the norm on the motionless periods.
# It has to be close to 9.81 if the computed parameters are satisfying.
g_after_calib = np.sqrt(np.mean(acc_imu[range(start[0], end[0]), 0])**2
+ np.mean(acc_imu[range(start[0], end[0]), 1])**2
+ np.mean(acc_imu[range(start[0], end[0]), 2])**2)
acc_norm = np.sqrt(np.sum(acc_imu[range(start[0], end[0]), i]**2
for i in range(0, 3)))
m_1 = np.mean(acc_norm)
s_1 = np.std(acc_norm)
print "norm_acc", g_after_calib, m_1, s_1
plt.plot(acc_norm)
if g_after_calib < 9.91 and g_after_calib > 9.71:
print "CALIB OK"
else:
print "CALIB KO"
#yield assert_equal, resp, True
return
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'))
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)