def test_q_vector(self):
result = quat.q_vector([cos(0.1), 0, 0, sin(0.1)])
correct = array([0., 0., 0.09983342])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.q_vector([[cos(0.1), 0., 0, sin(0.1)],
[cos(0.2), 0, sin(0.2), 0]])
correct = array([[0., 0., 0.09983342], [0., 0.19866933, 0.]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
def test_q_vector(self):
result = quat.q_vector([cos(0.1), 0, 0, sin(0.1)])
correct = array([ 0. , 0. , 0.09983342])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.q_vector([[cos(0.1), 0., 0, sin(0.1)],
[cos(0.2), 0, sin(0.2), 0]])
correct = array([[ 0. , 0. , 0.09983342],
[ 0. , 0.19866933, 0. ]])
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_kalman(self):
# Analyze the simulated data with "kalman"
imu = self.imu_signals
q_kalman = imus.kalman(imu['rate'], imu['gia'], imu['omega'],
imu['magnetic'])
# and then check, if the quat_vector = [0, sin(45), 0]
result = quat.q_vector(q_kalman[-1]) # [0, 0.46, 0]
correct = array([0., np.sin(np.deg2rad(45)), 0.]) # [0, 0.71, 0]
error = norm(result - correct)
self.assertAlmostEqual(
error, 0, places=2
) # It is not clear why the Kalman filter is not more accurate
# Get data
inFile = os.path.join(myPath, 'data', 'data_xsens.txt')
from skinematics.sensors.xsens import XSens
initialPosition = array([0, 0, 0])
R_initialOrientation = rotmat.R(0, 90)
sensor = XSens(in_file=inFile,
R_init=R_initialOrientation,
pos_init=initialPosition,
q_type='kalman')
print(sensor.source)
q = sensor.quat
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)
def test_analytical(self):
# Analyze the simulated data with "analytical"
imu = self.imu_signals
q, pos, vel = imus.analytical(R_initialOrientation=np.eye(3),
omega = imu['omega'],
initialPosition=np.zeros(3),
accMeasured = imu['gia'],
rate = imu['rate'])
# and then check, if the position is [0,0,0], and the orientation-quat = [0, sin(45), 0]
self.assertTrue(np.max(np.abs(pos[-1]))<0.001) # less than 1mm
result = quat.q_vector(q[-1])
correct = array([ 0., np.sin(np.deg2rad(45)), 0.])
error = norm(result-correct)
self.assertAlmostEqual(error, 0)
def test_analytical(self):
# Analyze the simulated data with "analytical"
imu = self.imu_signals
q, pos, vel = imus.analytical(R_initialOrientation=np.eye(3),
omega=imu['omega'],
initialPosition=np.zeros(3),
accMeasured=imu['gia'],
rate=imu['rate'])
# and then check, if the position is [0,0,0], and the orientation-quat = [0, sin(45), 0]
self.assertTrue(np.max(np.abs(pos[-1])) < 0.001) # less than 1mm
result = quat.q_vector(q[-1])
correct = array([0., np.sin(np.deg2rad(45)), 0.])
error = norm(result - correct)
self.assertAlmostEqual(error, 0)
def test_kalman(self):
# Analyze the simulated data with "kalman"
imu = self.imu_signals
q_kalman = imus.kalman(imu['rate'], imu['gia'], imu['omega'], imu['magnetic'])
# and then check, if the quat_vector = [0, sin(45), 0]
result = quat.q_vector(q_kalman[-1]) # [0.76, 0, 0]
correct = array([ 0., np.sin(np.deg2rad(45)), 0.]) # [0, 0.71, 0]
error = norm(result-correct)
self.assertAlmostEqual(error, 0, places=2) # It is not clear why the Kalman filter is not more accurate
# Get data
inFile = os.path.join(myPath, 'data', 'data_xsens.txt')
from skinematics.sensors.xsens import XSens
initialPosition = array([0,0,0])
R_initialOrientation = rotmat.R(0,90)
sensor = XSens(in_file=inFile, R_init = R_initialOrientation, pos_init = initialPosition, q_type='kalman')
print(sensor.source)
q = sensor.quat
print('\nInverted:')
pprint(q_inverted)
# Conjugation
q_conj = quat.q_conj(q_data)
print('\nConjugated:')
pprint(q_conj)
# Multiplication
q_multiplied = quat.q_mult(q_data, q_data)
print('\nMultiplied:')
pprint(q_multiplied)
# 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))