def test_qrotate(self):
v1 = np.array([[1, 0, 0], [1, 1, 0]])
v2 = np.array([[1, 1, 0], [2, 0, 0]])
correct = array([[0., 0., 0.38268343], [0., 0., -0.38268343]])
self.assertTrue(
np.all(
np.abs(vector.q_shortest_rotation(v1, v2) -
correct) < self.delta))
correct = array([0., 0., 0.38268343])
self.assertTrue(
norm(vector.q_shortest_rotation(v1[0], v2[0]) -
correct) < self.delta)
def get_init_orientation_sensor(resting_acc_vector):
"""
Calculates the approximate orientation of the gravity vector of an inertial measurement unit (IMU), assuming a
resting state and with respect to a global, space-fixed coordinate system.
Underlying idea: Upwards movements should be in positive z-direction, but in the experimental setting, the sensor's
z-axis is pointing to the right, so the provided acceleration vector is first aligned with a vector with the
gravitational constant as it's length lying on the sensor y-axis and pointing in positive direction and then rolled
90 degrees to the right along the x-axis
:param resting_acc_vector: acceleration vector from imu, should be pointing upwards, i.e. measured at rest.
:return: a vector around which a rotation of the sensor approximately aligns it's coordinate system with the global
"""
# create a vector with the gravitational constant as it's length in positive y-axis direction
g_vector = np.array([0, g, 0])
# determine the vector around which a rotation of the sensor would align it's own (not global) y-axis with gravity
sensor_orientation_g_aligned = vector.q_shortest_rotation(
resting_acc_vector, g_vector)
# create a rotation vector for a 90 degree rotation along the x-axis
roll_right = vector.q_shortest_rotation(np.array([0, 1, 0]),
np.array([0, 0, 1]))
# roll the sensor coordinate system to the right
sensor_orientation_global = quat.q_mult(roll_right,
sensor_orientation_g_aligned)
return sensor_orientation_global
def analytical(
R_initialOrientation=np.eye(3),
omega=np.zeros((5, 3)),
initialPosition=np.zeros(3),
initialVelocity=np.zeros(3),
accMeasured=np.column_stack((np.zeros((5, 2)), 9.81 * np.ones(5))),
rate=100):
''' 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
----------
R_initialOrientation: ndarray(3,3)
Rotation matrix describing the initial orientation of the sensor,
except a mis-orienation with respect to gravity
omega : ndarray(N,3)
Angular velocity, in [rad/s]
initialPosition : ndarray(3,)
initial Position, in [m]
accMeasured : ndarray(N,3)
Linear acceleration, in [m/s^2]
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]
vel : ndarray(N,3)
Velocity in space [m/s]
Example
-------
>>> q1, pos1 = analytical(R_initialOrientation, omega, initialPosition, acc, rate)
'''
if omega.ndim == 1:
raise ValueError('The input to "analytical" requires matrix inputs.')
# Transform recordings to angVel/acceleration in space --------------
# Orientation of \vec{g} with the sensor in the "R_initialOrientation"
g = constants.g
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)
q_initial = rotmat.convert(R_initialOrientation, to='quat')
# combine the two, to form a reference orientation. Note that the sequence
# is very important!
q_ref = quat.q_mult(q_initial, q0)
# 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=initialVelocity[ii])
pos[:, ii] = cumtrapz(vel[:, ii],
dx=1. / rate,
initial=initialPosition[ii])
return (q, pos, vel)
print('v2 normalized is {0}\n'.format(v2_normalized))
# Projection
projected = vector.project(v1, v2)
print('The projection of v1 onto v2 is {0}\n'.format(projected))
# Plane orientation
n = vector.plane_orientation(v0, v1, v2)
print('The plane spanned by v0, v1, and v2 is orthogonal to {0}\n'.format(n))
# Gram-Schmidt orthogonalization
gs = vector.GramSchmidt(v0, v1, v2)
print('The Gram-Schmidt orthogonalization of the points v0, v1, and v2 is {0}\n'.format(np.reshape(gs, (3,3))))
# Shortest rotation
q_shortest = vector.q_shortest_rotation(v1, v2)
print('The shortest rotation that brings v1 in alignment with v2 is described by the quaternion {0}\n'.format(q_shortest))
# Rotation of a vector by a quaternion
q = [0, 0.1, 0]
rotated = vector.rotate_vector(v1, q)
print('v1 rotated by {0} is: {1}'.format(q, rotated))
print('Done')
'''
Output
------
The length of v2 is 1.414
The angle between v1 and v2 is 45.0 degree