def calib_mag_param(data_calib_file):
""" Compute mag calibration parameters
"""
[offset_mag, scale_mag] = \
calib_mag.compute(data_calib_file)
return [offset_mag, scale_mag]
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 test1():
""" Test1
"""
from sensbiotk.io import iofox_deprec as fox
from sensbiotk.io import viz
print "Test Calibration"
[time, acc, mag, gyr] = fox.load_foximu_csvfile(FILETEST)
viz.plot_imu(0, "calib", time[:, 1], acc, mag, gyr)
acc_motionless = acc[5500:7500]
gyr_motionless = gyr[5500:7500]
mag_motion = mag[8000:18000]
scale, offset = calib_acc.compute_simple(acc_motionless)
print "Acc"
print "Scale=", scale
print "Offset=", offset
scale, offset = calib_mag.compute(mag_motion)
print "Mag"
print "Scale=", scale
print "Offset=", offset
scale, offset = calib_gyr.compute(gyr_motionless)
print "Gyr"
print "Scale=", scale
print "Offset=", offset
viz.plot_show()
return
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 calib_imu_parameters(acc, mag, gyr):
""" Compute IMU calibration parameters
Parameters :
------------
acc : numpy array of float
accelerometers data motionless containing all the acquisition
on three axis
mag : numpy array of float
magnetometers in motion containing all the acquisition
on three axis
gyr : numpy array of float
gyrometers data motionless containing all the acquisition
on three axis
Returns
-------
params : numpy array of float
[ [off_accx off_accy off_accz], offset for accelerometers
[sca_accx sca_accy sca_accz], scale for accelerometers
[off_magx off_magy off_magz], offset for magnetometers
[sca_magx sca_magy sca_magz], scale for magnetometers
[off_gyrx off_gyry off_gyrz], offset for gyrometers
[sca_gyrx sca_gyry sca_gyrz]] scale for gyrometers
"""
# Accelerometers calibration parameters
offset, scale = calib_acc.compute(acc)
params_acc = np.append([offset], [scale[0, :]], axis=0)
params_acc = np.append(params_acc, [scale[1, :]], axis=0)
params_acc = np.append(params_acc, [scale[2, :]], axis=0)
# Magnetometers calibration parameters
offset, scale = calib_mag.compute(mag)
params_mag = np.append([offset], [scale[0, :]], axis=0)
params_mag = np.append(params_mag, [scale[1, :]], axis=0)
params_mag = np.append(params_mag, [scale[2, :]], axis=0)
# Gyrometers calibration parameters
offset, scale = calib_gyr.compute(gyr)
params_gyr = np.append([offset], [scale[0]], axis=0)
params_gyr = np.append(params_gyr, [scale[1]], axis=0)
params_gyr = np.append(params_gyr, [scale[2]], axis=0)
return params_acc, params_mag, params_gyr
def real_time_madgwick():
rt = RT_Madgwick()
# instanciate dongle
foxdongle = fdongle.FoxDongle()
# init 3d visu
rt.f_3D_plot_init()
print 'Enter acquisition loop (type ^C to stop).'
init = False
calib_mag_bool = False
scale = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
offset = np.array([0, 0, 0])
while True:
try:
# handle dongle initialization
if not init and foxdongle.init_dongle():
init = True
print 'Device is connected to %s.' % (foxdongle.line())
if foxdongle.is_running():
data = foxdongle.read()
if data is not None and data.shape == (11, ):
data.reshape(1, 11)
data[5:8] = np.transpose(
np.dot(
scale,
np.transpose((data[5:8] - np.transpose(offset)))))
if not calib_mag_bool: # record 40s of data for the magnetometer calibration
print 'Calib magneto, 20 seconds \n'
data_calib = np.zeros((4000, 11))
for i in range(1, 4000):
data_calib[i] = foxdongle.read().reshape(1, 11)
time.sleep(0.005)
offset, scale = calib_mag.compute(data_calib[:, 5:8])
print scale
print offset
print 'Calib magneto over'
print '-----------------\n'
print 'Hold the IMU as on the VPython visualization (initial position)'
time.sleep(6)
print 'START'
rt.text.height = 0
calib_mag_bool = True
else:
rt.update(data[2:12])
time.sleep(0.005)
except KeyboardInterrupt:
print "\nStopped"
break
except Exception as e:
logging.error('exception reached:' + str(e))
# must close to kill read thread (fox_sink)
foxdongle.close_dongle()
print 'Done'
def real_time_martin():
rt = RT_Martin()
# instanciate dongle
foxdongle = fdongle.FoxDongle()
# init 3d visu
rt.f_3D_plot_init()
print 'Enter acquisition loop (type ^C to stop).'
init = False
init_observer = False
init_frame = False
calib_mag_bool = False
scale = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
offset = np.array([0, 0, 0])
while True:
try:
# handle dongle initialization
if not init and foxdongle.init_dongle():
init = True
print 'Device is connected to %s.' % (foxdongle.line())
if foxdongle.is_running():
data = foxdongle.read()
if data is not None and data.shape == (11, ):
data = data.reshape(1, 11)
data[0, 5:8] = np.transpose(
np.dot(
scale,
np.transpose(
(data[0, 5:8] - np.transpose(offset)))))
if not calib_mag_bool: # record 40s of data for the magnetometer calibration
print 'Calib magneto, 20 seconds \n'
data_calib = np.zeros((4000, 11))
for i in range(1, 4000):
data_calib[i] = foxdongle.read().reshape(1, 11)
time.sleep(0.005)
offset, scale = calib_mag.compute(data_calib[:, 5:8])
print scale
print offset
print 'Calib magneto over'
print 'Initializing the observer...'
calib_mag_bool = True
if not init_observer and calib_mag_bool: # record data for initializing the observer
print 'Please stay without moving in the initial position (as VPython)'
time.sleep(4)
data_obs = np.zeros((1000, 11))
for i in range(0, 1000):
data = foxdongle.read().reshape(1, 11)
data[0, 5:8] = np.transpose(
np.dot(
scale,
np.transpose((data[0, 5:8] -
np.transpose(offset)))))
data_obs[i, :] = data[:]
time.sleep(0.005)
print('.'),
rt.init_observer(np.mean(data_obs[:, 2:12], 0))
init_observer = True
print 'Start animation...'
rt.text.height = 0
if init_observer and not init_frame:
for i in range(
0, 1000): # waits that the algorithm converges
data = foxdongle.read().reshape(1, 11)
data[0, 5:8] = np.transpose(
np.dot(
scale,
np.transpose((data[0, 5:8] -
np.transpose(offset)))))
rt.update(data)
time.sleep(0.005)
rt.quat_offset = nq.conjugate(rt.quaternion)
init_frame = True
else:
rt.update(data)
time.sleep(0.005)
except KeyboardInterrupt:
print "\nStopped"
break
except Exception as e:
logging.error('exception reached:' + str(e))
# must close to kill read thread (fox_sink)
foxdongle.close_dongle()
print 'Done'