Exemple #1
0
    def init_observer(self, z):
        """ Martin Salaun init observer
        """
        # Quaternions construction
        ya = [0, z[0], z[1], z[2]]
        yb = [0, z[3], z[4], z[5]]
        yc = nq.mult(ya, yb)
        # Normalization
        ya = nq.normalize(ya)
        #        yb = nq.normalize(yb)
        yc[0] = 0
        yc = nq.normalize(yc)

        if ya[3] == 1:
            self.q = 1
            self.qinv = 1
        else:
            self.qinv = [-ya[2], 1 - ya[3], 0, ya[1]]
            self.qinv = nq.normalize(self.qinv)
            self.q = nq.conjugate(self.qinv)

        yc = nq.mult(nq.mult(self.q, yc), self.qinv)

        if yc[2] != 1:
            self.qinv = nq.mult(self.qinv, [-yc[1], 0, yc[3], 1 - yc[2]])
            self.qinv = nq.normalize(self.qinv)
            self.q = nq.conjugate(self.qinv)

        return self.q
Exemple #2
0
def compute(q_0, q_1, q_offset=[1, 0, 0, 0]):
    """ Computes rotation angle between two quaternions

    Parameters :
    ------------
    q_0  : numpy array of float
                 the first quaternion (w, x, y, z)
    q_1 : numpy array of float
                 the second quaternion (w, x, y, z)
    q_offset : numpy array of float
                 the initial quaternion corresponding to the offset
                 between q_0 and q_1 at an initial position
                 (can be generated using sensbiotk.calib.calib_geom)

    Returns
    -------
     angle : float
             the angle of rotation between q_0 and q_1 (rad)
     rot_axis : numpy array of float
             the axis of rotation

    """

    q_corr = nq.mult(
        nq.mult(np.transpose(nq.conjugate(q_0)), np.transpose(q_1)),
        nq.conjugate(q_offset))  # q_corr = conj(q_0) x q_1 x conj(q_offset)
    angle = np.arccos(q_corr[0]) * 2  # angle = acos(q_corr(w)) x 2
    rot_axis = q_corr[1:4]  # the axis of rotation between
    # the two quaternions (ex: if along Z, should be close to [0, 0, 1])

    return angle, rot_axis
Exemple #3
0
def compute(data_imu0, data_imu1):
    """ Compute an "offset" quaternion between two supposed aligned quaternions
    
    Parameters :
    ------------
    data_imu0  : numpy array of float
                 the IMU column stacked reference position data [ACC MAG GYR]
    data_imu1 : numpy array of float
                 the IMU column stacked reference position data [ACC MAG GYR]

    Returns
    -------
    q_offset : quaternion [w, x, y, z]
                 the offset quaternion between IMU0 and IMU1
    """

    # Averages the data on the reference position
    data_ref_imu0 = np.mean(data_imu0[:, 0:10], 0)
    data_ref_imu1 = np.mean(data_imu1[:, 0:10], 0)

    # madgwick calculation
    #    q_init=[1, 0, 0, 0]
    #    q0 = madgwick.update(q_init, data_ref_imu0)
    #    q1 = madgwick.update(q_init, data_ref_imu1)

    # Uses martin salaun for calculating the two quaternions
    init0 = martin.martin_ahrs()
    init1 = martin.martin_ahrs()
    q0 = init0.init_observer(data_ref_imu0)
    q1 = init1.init_observer(data_ref_imu1)

    q_offset = nq.mult(np.transpose(nq.conjugate(q0)),
                       np.transpose(q1))  # q_offset = conj(q0) x q1

    return q_offset
Exemple #4
0
def update(q, z, fs=200):

    accelero = z[0:3]
    magneto = z[3:6]
    gyro = z[6:9]

    sample_period = 1. / fs
    Kp = 0.01  # algorithm proportional gain
    Ki = 0  # algorithm integral gain

    # Normalise accelerometer measurement
    norm_accelero = norm(accelero)
    if norm_accelero != 0:
        accelero = accelero / norm_accelero
    # Normalise magnetometer measurement
    norm_magneto = norm(magneto)
    if norm_magneto != 0:
        magneto = magneto / norm_magneto
    # Reference direction of Earth's magnetic field
    h = nq.mult(q, nq.mult(np.concatenate(([0], magneto)), nq.conjugate(q)))
    b = np.array([0, np.linalg.norm([h[1], h[2]]), 0, h[3]])

    # Estimated direction of gravity and magnetic field
    v = np.array([[2 * (q[1] * q[3] - q[0] * q[2])],
                  [2 * (q[0] * q[1] + q[2] * q[3])],
                  [q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2]])
    w = np.array([[
        2 * b[1] * (0.5 - q[2]**2 - q[3]**2) + 2 * b[3] *
        (q[1] * q[3] - q[0] * q[2])
    ],
                  [
                      2 * b[1] * (q[1] * q[2] - q[0] * q[3]) + 2 * b[3] *
                      (q[0] * q[1] + q[2] * q[3])
                  ],
                  [
                      2 * b[1] * (q[0] * q[2] + q[1] * q[3]) + 2 * b[3] *
                      (0.5 - q[1]**2 - q[2]**2)
                  ]])

    # Error is sum of cross product between
    # estimated direction and measured direction of fields
    e = np.cross(np.transpose(accelero), np.transpose(v)) + np.cross(
        np.transpose(magneto), np.transpose(w))

    if (Ki > 0):
        obj.eInt = obj.eInt + e * sample_period

    # Apply feedback terms
    gyro = np.transpose(gyro + Kp * e + Ki * obj.eInt)

    # Compute rate of change of quaternion
    q_dot = np.dot(0.5, nq.mult(q, (0, gyro[0], gyro[1], gyro[2])))

    # Integrate to yield quaternion
    q = q + np.transpose((q_dot * sample_period))

    return q / np.linalg.norm(q)
Exemple #5
0
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)')
Exemple #6
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"
Exemple #7
0
 def compute_values(self):
     while self.run:
         try:
             quat = euler.euler2quat(self.heading*np.pi/180,self.attitude*np.pi/180,self.bank*np.pi/180)
             if self.quat_offset == [0, 0, 0]:
                 self.quat_offset = quat
                 print('======')
                 print(str(self.quat_offset))
             else:
                 quat = q.mult(q.conjugate(self.quat_offset),quat)
             self.rad_pedal_angle = euler.quat2euler2(quat)[0]
             self.pedal_angle = (-self.rad_pedal_angle*180/np.pi)
             if self.pedal_angle < 0 :
                 self.pedal_angle = self.pedal_angle + 360
             self.plot()
             time.sleep(0.010)
             #save computed values in file
             if self.rec == True:
                 self.data_out.write('%.1f \t %.1f \t %.1f \t %.1f \t %.1f \t %.1f \t %.1f \t %.1f \n' %(self.time_clock_imu1 - self.offset_time, self.pedal_angle, self.bike_speed, self.distance, ((self.gyr_z*60)/(2*np.pi)), self.hr_ptap.power, self.hr_ptap.total_power, self.hr_ptap.hr))
         
         except:
             continue
Exemple #8
0
    def update(self, z, sample_period):
        """ Martin Salaun iteration observer
        """

        # Quaternions construction
        ya = np.array([0, z[0], z[1], z[2]])
        #        ya = nq.normalize(ya)
        yb = np.array([0, z[3], z[4], z[5]])
        #        yb = nq.normalize(yb)
        wm = np.array([0, z[6], z[7], z[8]])
        #        wm = nq.normalize(wm)

        # Compute quaternions products
        yc = nq.mult(ya, yb)
        yc[0] = 0
        yd = nq.mult(yc, ya)
        yd[0] = 0

        # Compute errors
        EA = self._A - \
            nq.mult(self.q, nq.mult(ya, self.qinv)) / self.a_s
        EC = self._C - \
            nq.mult(self.q, nq.mult(yc, self.qinv)) / self.c_s
        ED = self._D - \
            nq.mult(self.q, nq.mult(yd, self.qinv)) / (self.a_s * self.c_s)

        # Numerical stabilisation
        EA[0] = 0
        EC[0] = 0
        ED[0] = 0

        # sEA = <EA, EA - A> = ||EA||² - <EA, A>
        sEA = nq.norm(EA) - EA[3]
        # sEC = <EC, EC - C> = ||EC||² - <EC, C>
        sEC = nq.norm(EC) - EC[2]
        # sED = <ED, ED - D> = ||ED||² - <ED, D>
        sED = nq.norm(ED) - ED[1]

        LE = nq.mult(self._A, EA) * self.la + nq.mult(self._C, EC) * self.lc \
            + nq.mult(self._D, ED) * self.ld
        LE[0] = 0
        ME = LE * (-self.sigma)

        if self.la + self.ld != 0:
            NE = self.n / (self.la + self.ld) * (self.la * sEA + self.ld * sED)
        else:
            NE = 0

        if self.lc + self.ld != 0:
            OE = self.o / (self.lc + self.ld) * (self.lc * sEC + self.ld * sED)
        else:
            OE = 0

        qdot = nq.mult(self.q, wm - self.wb) * 0.5 + \
            nq.mult(LE, self.q) + self.q * (self.k * (1 - nq.norm(self.q)))
        wbdot = nq.mult(nq.mult(self.qinv, ME), self.q)
        a_sdot = self.a_s * NE
        csdot = self.c_s * OE

        # Integration
        self.q = self.q + qdot * sample_period
        self.wb = self.wb + wbdot * sample_period
        self.a_s = self.a_s + a_sdot * sample_period
        self.c_s = self.c_s + csdot * sample_period

        # compute inverse for the next step
        self.qinv = nq.conjugate(self.q)

        qrot = nq.mult([0, 1, 0, 0], self.q)

        return qrot
Exemple #9
0
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)
Exemple #10
0
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)
Exemple #11
0
def update(q, z, fs=200):
    """
    Updates the computed q quaternion
    """

    accelero = z[0:3]
    magneto = z[3:6]
    gyro = z[6:9]

    sample_period = 1. / fs
    Beta = 0.02

    # Normalise accelerometer measurement
    norm_accelero = norm(accelero)
    if norm_accelero != 0:
        accelero = accelero / norm_accelero
    # Normalise magnetometer measurement
    norm_magneto = norm(magneto)
    if norm_magneto != 0:
        magneto = magneto / norm_magneto
    # Reference direction of Earth's magnetic field
    h = nq.mult(q, nq.mult(np.concatenate(([0], magneto)), nq.conjugate(q)))
    #    h = [1, 1, 1, 1]
    b = np.array([0, np.linalg.norm([h[1], h[2]]), 0, h[3]])
    # Gradient decent algorithm corrective step
    F = np.array([[2 * (q[1] * q[3] - q[0] * q[2]) - accelero[0]],
                  [2 * (q[0] * q[1] + q[2] * q[3]) - accelero[1]],
                  [2 * (0.5 - q[1]**2 - q[2]**2) - accelero[2]],
                  [
                      2 * b[1] * (0.5 - q[2]**2 - q[3]**2) + 2 * b[3] *
                      (q[1] * q[3] - q[0] * q[2]) - magneto[0]
                  ],
                  [
                      2 * b[1] * (q[1] * q[2] - q[0] * q[3]) + 2 * b[3] *
                      (q[0] * q[1] + q[2] * q[3]) - magneto[1]
                  ],
                  [
                      2 * b[1] * (q[0] * q[2] + q[1] * q[3]) + 2 * b[3] *
                      (0.5 - q[1]**2 - q[2]**2) - magneto[2]
                  ]])

    J = np.array([[-2 * q[2], 2 * q[3], -2 * q[0], 2 * q[1]],
                  [2 * q[1], 2 * q[0], 2 * q[3], 2 * q[2]],
                  [0, -4 * q[1], -4 * q[2], 0],
                  [
                      -2 * b[3] * q[2], 2 * b[3] * q[3],
                      4 * b[1] * q[2] - 2 * b[3] * q[0],
                      -4 * b[1] * q[3] + 2 * b[3] * q[1]
                  ],
                  [
                      -2 * b[1] * q[3] + 2 * b[3] * q[1],
                      2 * b[1] * q[2] + 2 * b[3] * q[0],
                      2 * b[1] * q[1] + 2 * b[3] * q[3],
                      -2 * b[1] * q[0] + 2 * b[3] * q[2]
                  ],
                  [
                      2 * b[1] * q[2], 2 * b[1] * q[3] - 4 * b[3] * q[1],
                      2 * b[1] * q[0] - 4 * b[3] * q[2], 2 * b[1] * q[1]
                  ]])

    step = (np.dot(np.transpose(J), F))
    # normalise step magnitude
    step = step / norm(step)

    # Compute rate of change of quaternion
    q_dot = np.dot(
    0.5, nq.mult(q, (0, gyro[0], gyro[1], gyro[2]))) - \
    np.dot(Beta, np.transpose(step))

    # Integrate to yield quaternion
    q = q + (q_dot * sample_period)

    return q / np.linalg.norm(q)
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)
Exemple #13
0
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)
Exemple #15
0
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'
Exemple #16
0
def test_conjugate():
    # Takes sequence
    cq = nq.conjugate((1, 0, 0, 0))
    # Returns float type
    yield assert_true, cq.dtype.kind == 'f'