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 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 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 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 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(°)'))
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)