def test_Euler(self):
testmat = np.array([[np.sqrt(2)/2, -np.sqrt(2)/2, 0],
[np.sqrt(2)/2, np.sqrt(2)/2, 0],
[0, 0, 1]])
Euler = rotmat.sequence(testmat, to ='Euler')
correct = np.r_[[np.pi/4,0,0]]
self.assertAlmostEqual(np.linalg.norm(correct - np.array(Euler)), 0)
alpha, beta, gamma = 10, 20, 30
mat_euler = rotmat.R(2, gamma) @ rotmat.R(0, beta) @ rotmat.R(2, alpha)
angles_euler = np.rad2deg(rotmat.sequence(mat_euler, to='Euler'))
correct = np.r_[alpha, beta, gamma]
self.assertAlmostEqual(np.linalg.norm(correct - np.array(angles_euler)), 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, 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):
# 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='mahony')
q = sensor.quat
def test_set_qtype(self):
"""Tests if the test crashes on any of the existing qtype options"""
# 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')
allowed_values = ['analytical', 'kalman', 'madgwick', 'mahony', None]
for sensor_type in allowed_values:
sensor.set_qtype(sensor_type)
print('{0} is running'.format(sensor_type))
def test_R_axis2(self):
R2 = np.array([[np.sqrt(2)/2, -np.sqrt(2)/2, 0],
[np.sqrt(2)/2, np.sqrt(2)/2, 0],
[0, 0, 1]])
self.assertTrue(np.all(np.abs(R2 - rotmat.R(2, 45))< self.delta))
def test_R_axis1(self):
R1 = np.array([[ np.sqrt(2)/2, 0, np.sqrt(2)/2],
[0, 1, 0],
[-np.sqrt(2)/2, 0, np.sqrt(2)/2]])
self.assertTrue(np.all(np.abs(R1 - rotmat.R(1, 45))< self.delta))
def test_R_axis0(self):
R0 = np.array([[1,0,0],
[0, np.sqrt(2)/2, -np.sqrt(2)/2],
[0, np.sqrt(2)/2, np.sqrt(2)/2]])
self.assertTrue(np.all(np.abs(R0 - rotmat.R(0, 45))< self.delta))