def test_convert(self):
result = quat.convert(array([0, 0, 0.1]), to='rotmat')
correct = array([[0.98, -0.19899749, 0.], [0.19899749, 0.98, 0.],
[0., 0., 1.]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.convert([[0, 0, 0.1], [0, 0.1, 0]], to='rotmat')
correct = array(
[[0.98, -0.19899749, 0., 0.19899749, 0.98, 0., 0., 0., 1.],
[0.98, 0., 0.19899749, 0., 1., 0., -0.19899749, 0., 0.98]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
def test_convert(self):
result = quat.convert(array([0, 0, 0.1]), to='rotmat')
correct = array([[ 0.98 , -0.19899749, 0. ],
[ 0.19899749, 0.98 , 0. ],
[ 0. , 0. , 1. ]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.convert([[0, 0, 0.1], [0, 0.1, 0]], to='rotmat')
correct = array([[ 0.98 , -0.19899749, 0. , 0.19899749, 0.98 ,
0. , 0. , 0. , 1. ],
[ 0.98 , 0. , 0.19899749, 0. , 1. ,
0. , -0.19899749, 0. , 0.98 ]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
def test_madgwick(self):
from skinematics.sensors.manual import MyOwnSensor
## Get data
imu = self.imu_signals
in_data = {'rate' : imu['rate'],
'acc' : imu['gia'],
'omega' : imu['omega'],
'mag' : imu['magnetic']}
my_sensor = MyOwnSensor(in_file='Simulated sensor-data', in_data=in_data,
R_init = quat.convert(self.q_init, to='rotmat'),
pos_init = self.pos_init,
q_type = 'madgwick')
# and then check, if the quat_vector = [0, sin(45), 0]
q_madgwick = my_sensor.quat
result = quat.q_vector(q_madgwick[-1])
correct = array([ 0., np.sin(np.deg2rad(45)), 0.])
error = norm(result-correct)
#self.assertAlmostEqual(error, 0)
self.assertTrue(error < 1e-3)
def test_mahony(self):
from skinematics.sensors.manual import MyOwnSensor
## Get data
imu = self.imu_signals
in_data = {
'rate': imu['rate'],
'acc': imu['gia'],
'omega': imu['omega'],
'mag': imu['magnetic']
}
my_sensor = MyOwnSensor(in_file='Simulated sensor-data',
in_data=in_data,
R_init=quat.convert(self.q_init, to='rotmat'),
pos_init=self.pos_init,
q_type='mahony')
# and then check, if the quat_vector = [0, sin(45), 0]
q_mahony = my_sensor.quat
result = quat.q_vector(q_mahony[-1])
correct = array([0., np.sin(np.deg2rad(45)), 0.])
error = norm(result - correct)
#self.assertAlmostEqual(error, 0)
self.assertTrue(error < 1e-3)
async def data_loop(bmx: BMX160, q, pos, vel, prev_time):
# Read IMU data and place into matrices to be analyzed in batches
accel, gyro, magn, samp_period, time, prev_time1 = get_imu_data(
bmx, prev_time)
samp_freq = 1 / (samp_period)
# print(q[119])
# Convert initial rotation quaternion into rotation matrix
initial_rot = quat.convert(q[NUM_SAMPLES - 1],
to='rotmat') # .export(to='rotmat')
# print(q[NUM_SAMPLES - 1])
# # Use analytical method to calculate position, orientation and velocity
# (q1, pos1, vel1) = imu_calc.calc_orientation_position(omega = gyro,
# accMeasured = accel,
# rate = samp_freq,
# initialOrientation = initial_rot,
# initialPosition = pos[NUM_SAMPLES - 1],
# initialVelocity= vel[NUM_SAMPLES - 1],
# timeVector=time)
# Use Kalmans method to calculate orientation, and position
q1 = imu_calc.kalman(rate=samp_freq,
acc=accel,
omega=gyro,
mag=magn,
initialPosition=pos[NUM_SAMPLES - 1],
initialVelocity=vel[NUM_SAMPLES - 1],
timeVector=time)
return q1, prev_time1
def test_convert(self):
result = rotmat.convert(quat.convert([0, 0, 0.1], to ='rotmat'), to ='quat')
correct = np.array([[ 0.99498744, 0. , 0. , 0.1 ]])
error = np.linalg.norm(result - correct)
self.assertTrue(error < self.delta)
def test_convert(self):
result = rotmat.convert(quat.convert([0, 0, 0.1], to ='rotmat'), to ='quat')
correct = np.array([[ 0.99498744, 0. , 0. , 0.1 ]])
error = np.linalg.norm(result - correct)
self.assertTrue(error < self.delta)
def analytical(R_initialOrientation, omega, initialPosition, accMeasured,
rate):
''' Reconstruct position and orientation with an analytical solution,
from angular velocity and linear acceleration.
Assumes a start in a stationary position. No compensation for drift.
Parameters
----------
omega : ndarray(N,3)
Angular velocity, in [rad/s]
accMeasured : ndarray(N,3)
Linear acceleration, in [m/s^2]
initialPosition : ndarray(3,)
initial Position, in [m]
R_initialOrientation: ndarray(3,3)
Rotation matrix describing the initial orientation of the sensor,
except a mis-orienation with respect to gravity
rate : float
sampling rate, in [Hz]
Returns
-------
q : ndarray(N,3)
Orientation, expressed as a quaternion vector
pos : ndarray(N,3)
Position in space [m]
Example
-------
>>> q1, pos1 = analytical(R_initialOrientation, omega, initialPosition, acc, rate)
'''
# Transform recordings to angVel/acceleration in space --------------
# Orientation of \vec{g} with the sensor in the "R_initialOrientation"
g = 9.81
g0 = np.linalg.inv(R_initialOrientation).dot(np.r_[0, 0, g])
# for the remaining deviation, assume the shortest rotation to there
q0 = vector.q_shortest_rotation(accMeasured[0], g0)
R0 = quat.convert(q0, 'rotmat')
# combine the two, to form a reference orientation. Note that the sequence
# is very important!
R_ref = R_initialOrientation.dot(R0)
q_ref = quat.rotmat2quat(R_ref)
# Calculate orientation q by "integrating" omega -----------------
q = quat.calc_quat(omega, q_ref, rate, 'bf')
# Acceleration, velocity, and position ----------------------------
# From q and the measured acceleration, get the \frac{d^2x}{dt^2}
g_v = np.r_[0, 0, g]
accReSensor = accMeasured - vector.rotate_vector(g_v, quat.q_inv(q))
accReSpace = vector.rotate_vector(accReSensor, q)
# Make the first position the reference position
q = quat.q_mult(q, quat.q_inv(q[0]))
# compensate for drift
#drift = np.mean(accReSpace, 0)
#accReSpace -= drift*0.7
# Position and Velocity through integration, assuming 0-velocity at t=0
vel = np.nan * np.ones_like(accReSpace)
pos = np.nan * np.ones_like(accReSpace)
for ii in range(accReSpace.shape[1]):
vel[:, ii] = cumtrapz(accReSpace[:, ii], dx=1. / rate, initial=0)
pos[:, ii] = cumtrapz(vel[:, ii],
dx=1. / rate,
initial=initialPosition[ii])
return (q, pos)
# Scalar and vector part
q_scalar = quat.q_scalar(q_data)
q_vector = quat.q_vector(q_data)
print('\nScalar part:')
pprint(q_scalar)
print('Vector part:')
pprint(q_vector)
# Convert to axis angle
q_axisangle = quat.quat2deg(q_unit)
print('\nAxis angle:')
pprint(q_axisangle)
# Conversion to a rotation matrix
rotmats = quat.convert(q_unit)
print('\nFirst rotation matrix')
pprint(rotmats[0].reshape(3, 3))
# Working with Quaternion objects
# -------------------------------
data = np.array([[0, 0, 0.1], [0, 0.2, 0]])
data2 = np.array([[0, 0, 0.1], [0, 0, 0.1]])
eye = quat.Quaternion(data)
head = quat.Quaternion(data2)
# Quaternion multiplication, ...
gaze = head * eye
# ..., division, ...