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 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 analytical(R_initialOrientation=np.eye(3),
omega=np.zeros((5, 3)),
initialPosition=np.zeros(3),
accMeasured=np.column_stack((np.zeros((5, 2)), g * np.ones(5))),
rate=128):
####################################################################################
# #
# github.com/thomas-haslwanter/scikit-kinematics/blob/master/skinematics/imus.py #
# Analytically reconstructing accmtr. position and orientation, using angular #
# velocity and linear acceleration. Assumes a start in a stationary position. #
# Needs auxiliary libraries - quat.py, vector.py, rotmat.py. #
# Parameters #
# ------------------------------------------------------------------------------ #
# R_initialOrientation : ndarray(3,3) --------- Rotation matrix describing #
# the sensor's initial orientation, except for a mis-orientation w/rt 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 - quaternion vector #
# pos : ndarray(N,3) -------------------------- Position in space [m] #
# vel : ndarray(N,3) -------------------------- Velocity in space [m/s] #
# #
####################################################################################
# Transform recordings to angVel/acceleration in space -------------------------
# ----- Find gravity's orientation on the sensor in "R_initialOrientation" -----
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. -----------------------
q_ref = quat.q_mult(q_initial, q0)
# Compute orientation q by "integrating" omega ---------------------------------
q = quat.calc_quat(omega, q_ref, rate, 'bf')
# Acceleration, velocity, and position -----------------------------------------
# ----- Using 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]))
# Done. ------------------------------------------------------------------------
return q
def analytical(
R_initialOrientation=np.eye(3),
omega=np.zeros((5, 3)),
initialPosition=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)
'''
# 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=0)
pos[:, ii] = cumtrapz(vel[:, ii],
dx=1. / rate,
initial=initialPosition[ii])
return (q, pos, vel)
def analytical(R_initialOrientation=np.eye(3),
omega=np.zeros((5,3)),
initialPosition=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)
'''
# 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=0)
pos[:,ii] = cumtrapz(vel[:,ii], dx=1./rate, initial=initialPosition[ii])
return (q, pos, vel)