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))