def test_q_mult(self):
result = quat.q_mult(self.qz, self.qz)
correct = array([ 0.98006658, 0. , 0. , 0.19866933])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.q_mult(self.qz, self.qy)
correct = array([ 0.99003329, -0.00996671, 0.09933467, 0.09933467])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.q_mult(self.quatMat, self.quatMat)
correct = array([[ 0.98006658, 0. , 0. , 0.19866933],
[ 0.98006658, 0. , 0.19866933, 0. ]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.q_mult(self.quatMat, self.qz)
correct = array([[ 0.98006658, 0. , 0. , 0.19866933],
[ 0.99003329, 0.00996671, 0.09933467, 0.09933467]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.q_mult(self.q3x, self.q3x)
correct = array([ 0.19866933, 0. , 0. ])
error = norm(result - correct)
self.assertTrue(error < self.delta)
def test_q_mult(self):
result = quat.q_mult(self.qz, self.qz)
correct = array([0.98006658, 0., 0., 0.19866933])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.q_mult(self.qz, self.qy)
correct = array([0.99003329, -0.00996671, 0.09933467, 0.09933467])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.q_mult(self.quatMat, self.quatMat)
correct = array([[0.98006658, 0., 0., 0.19866933],
[0.98006658, 0., 0.19866933, 0.]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.q_mult(self.quatMat, self.qz)
correct = array([[0.98006658, 0., 0., 0.19866933],
[0.99003329, 0.00996671, 0.09933467, 0.09933467]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.q_mult(self.q3x, self.q3x)
correct = array([0.19866933, 0., 0.])
error = norm(result - correct)
self.assertTrue(error < self.delta)
def rotate_vector(vector, q):
'''
Rotates a vector, according to the given quaternions.
Note that a single vector can be rotated into many orientations;
or a row of vectors can all be rotated by a single quaternion.
Parameters
----------
vector : array, shape (3,) or (N,3)
vector(s) to be rotated.
q : array_like, shape ([3,4],) or (N,[3,4])
quaternions or quaternion vectors.
Returns
-------
rotated : array, shape (3,) or (N,3)
rotated vector(s)
.. image:: ../docs/Images/vector_rotate_vector.png
:scale: 33%
Notes
-----
.. math::
q \\circ \\left( {\\vec x \\cdot \\vec I} \\right) \\circ {q^{ - 1}} = \\left( {{\\bf{R}} \\cdot \\vec x} \\right) \\cdot \\vec I
More info under
http://en.wikipedia.org/wiki/Quaternion
Examples
--------
>>> mymat = eye(3)
>>> myVector = r_[1,0,0]
>>> quats = array([[0,0, sin(0.1)],[0, sin(0.2), 0]])
>>> quat.rotate_vector(myVector, quats)
array([[ 0.98006658, 0.19866933, 0. ],
[ 0.92106099, 0. , -0.38941834]])
>>> quat.rotate_vector(mymat, [0, 0, sin(0.1)])
array([[ 0.98006658, 0.19866933, 0. ],
[-0.19866933, 0.98006658, 0. ],
[ 0. , 0. , 1. ]])
'''
vector = np.atleast_2d(vector)
qvector = np.hstack((np.zeros((vector.shape[0], 1)), vector))
vRotated = quat.q_mult(q, quat.q_mult(qvector, quat.q_inv(q)))
vRotated = vRotated[:, 1:]
if min(vRotated.shape) == 1:
vRotated = vRotated.ravel()
return vRotated
def test_q_inv(self):
result = quat.q_mult(self.qz, quat.q_inv(self.qz))
correct = array([1., 0., 0., 0.])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.q_mult(self.quatMat, quat.q_inv(self.quatMat))
correct = array([[1., 0., 0., 0.], [1., 0., 0., 0.]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.q_mult(self.q3x, quat.q_inv(self.q3x))
correct = array([0., 0., 0.])
error = norm(result - correct)
self.assertTrue(error < self.delta)
def test_q_inv(self):
result = quat.q_mult(self.qz, quat.q_inv(self.qz))
correct = array([ 1., 0., 0., 0.])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.q_mult(self.quatMat, quat.q_inv(self.quatMat))
correct = array([[ 1., 0., 0., 0.],
[ 1., 0., 0., 0.]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.q_mult(self.q3x, quat.q_inv(self.q3x))
correct = array([ 0., 0., 0.])
error = norm(result - correct)
self.assertTrue(error < self.delta)
def Update(self, Gyroscope, Accelerometer, Magnetometer):
'''Calculate the best quaternion to the given measurement values.'''
q = self.Quaternion
# short name local variable for readability
# Reference direction of Earth's magnetic field
h = vector.rotate_vector(Magnetometer, q)
b = np.hstack((0, np.sqrt(h[0]**2 + h[1]**2), 0, h[2]))
# Gradient decent algorithm corrective step
F = [
2 * (q[1] * q[3] - q[0] * q[2]) - Accelerometer[0],
2 * (q[0] * q[1] + q[2] * q[3]) - Accelerometer[1],
2 * (0.5 - q[1]**2 - q[2]**2) - Accelerometer[2],
2 * b[1] * (0.5 - q[2]**2 - q[3]**2) + 2 * b[3] *
(q[1] * q[3] - q[0] * q[2]) - Magnetometer[0],
2 * b[1] * (q[1] * q[2] - q[0] * q[3]) + 2 * b[3] *
(q[0] * q[1] + q[2] * q[3]) - Magnetometer[1],
2 * b[1] * (q[0] * q[2] + q[1] * q[3]) + 2 * b[3] *
(0.5 - q[1]**2 - q[2]**2) - Magnetometer[2]
]
J = np.array([[-2 * q[2], 2 * q[3], -2 * q[0], 2 * q[1]],
[2 * q[1], 2 * q[0], 2 * q[3], 2 * q[2]],
[0, -4 * q[1], -4 * q[2], 0],
[
-2 * b[3] * q[2], 2 * b[3] * q[3],
-4 * b[1] * q[2] - 2 * b[3] * q[0],
-4 * b[1] * q[3] + 2 * b[3] * q[1]
],
[
-2 * b[1] * q[3] + 2 * b[3] * q[1],
2 * b[1] * q[2] + 2 * b[3] * q[0],
2 * b[1] * q[1] + 2 * b[3] * q[3],
-2 * b[1] * q[0] + 2 * b[3] * q[2]
],
[
2 * b[1] * q[2], 2 * b[1] * q[3] - 4 * b[3] * q[1],
2 * b[1] * q[0] - 4 * b[3] * q[2], 2 * b[1] * q[1]
]])
step = J.T.dot(F)
step = vector.normalize(step) # normalise step magnitude
# Compute rate of change of quaternion
qDot = 0.5 * quat.q_mult(q, np.hstack([0, Gyroscope
])) - self.Beta * step
# Integrate to yield quaternion
q = q + qDot * self.SamplePeriod
self.Quaternion = vector.normalize(q).flatten()
def Update(self, Gyroscope, Accelerometer, Magnetometer):
'''Calculate the best quaternion to the given measurement values.'''
q = self.Quaternion
# short name local variable for readability
# Reference direction of Earth's magnetic field
h = vector.rotate_vector(Magnetometer, q)
b = np.hstack((0, np.sqrt(h[0]**2 + h[1]**2), 0, h[2]))
# Estimated direction of gravity and magnetic field
v = np.array([
2 * (q[1] * q[3] - q[0] * q[2]), 2 * (q[0] * q[1] + q[2] * q[3]),
q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2
])
w = np.array([
2 * b[1] * (0.5 - q[2]**2 - q[3]**2) + 2 * b[3] *
(q[1] * q[3] - q[0] * q[2]),
2 * b[1] * (q[1] * q[2] - q[0] * q[3]) + 2 * b[3] *
(q[0] * q[1] + q[2] * q[3]), 2 * b[1] *
(q[0] * q[2] + q[1] * q[3]) + 2 * b[3] * (0.5 - q[1]**2 - q[2]**2)
])
# Error is sum of cross product between estimated direction and measured direction of fields
e = np.cross(Accelerometer, v) + np.cross(Magnetometer, w)
if self.Ki > 0:
self._eInt += e * self.SamplePeriod
else:
self._eInt = np.array([0, 0, 0], dtype=np.float)
# Apply feedback terms
Gyroscope += self.Kp * e + self.Ki * self._eInt
# Compute rate of change of quaternion
qDot = 0.5 * quat.q_mult(q, np.hstack([0, Gyroscope])).flatten()
# Integrate to yield quaternion
q += qDot * self.SamplePeriod
self.Quaternion = vector.normalize(q)
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 kalman(rate,
acc,
omega,
mag,
D=[0.4, 0.4, 0.4],
tau=[0.5, 0.5, 0.5],
Q_k=None,
R_k=None,
initialPosition=np.zeros(3),
initialVelocity=np.zeros(3),
initialOrientation=np.zeros(3),
timeVector=np.zeros((5, 1)),
accMeasured=np.column_stack((np.zeros((5, 2)), 9.81 * np.ones(5))),
referenceOrientation=np.array([1., 0., 0., 0.])):
'''
Calclulate the orientation from IMU magnetometer data.
Parameters
----------
rate : float
sample rate [Hz]
acc : (N,3) ndarray
linear acceleration [m/sec^2]
omega : (N,3) ndarray
angular velocity [rad/sec]
mag : (N,3) ndarray
magnetic field orientation
D : (,3) ndarray
noise variance, for x/y/z [rad^2/sec^2]
parameter for tuning the filter; defaults from Yun et al.
can also be entered as list
tau : (,3) ndarray
time constant for the process model, for x/y/z [sec]
parameter for tuning the filter; defaults from Yun et al.
can also be entered as list
Q_k : None, or (7,7) ndarray
covariance matrix of process noises
parameter for tuning the filter
If set to "None", the defaults from Yun et al. are taken!
R_k : None, or (7,7) ndarray
covariance matrix of measurement noises
parameter for tuning the filter; defaults from Yun et al.
If set to "None", the defaults from Yun et al. are taken!
Returns
-------
qOut : (N,4) ndarray
unit quaternion, describing the orientation relativ to the coordinate
system spanned by the local magnetic field, and gravity
pos : (N,3) ndarray
Position array
vel : (N,3) ndarray
Velocity
Notes
-----
Based on "Design, Implementation, and Experimental Results of a Quaternion-
Based Kalman Filter for Human Body Motion Tracking" Yun, X. and Bachman,
E.R., IEEE TRANSACTIONS ON ROBOTICS, VOL. 22, 1216-1227 (2006)
'''
numData = len(acc)
# Set parameters for Kalman Filter
tstep = 1. / rate
# check input
assert len(tau) == 3
tau = np.array(tau)
# Initializations
x_k = np.zeros(7) # state vector
z_k = np.zeros(7) # measurement vector
z_k_pre = np.zeros(7)
P_k = np.eye(7) # error covariance matrix P_k
Phi_k = np.eye(7) # discrete state transition matrix Phi_k
for ii in range(3):
Phi_k[ii, ii] = np.exp(-tstep / tau[ii])
H_k = np.eye(7) # Identity matrix
D = np.r_[0.4, 0.4, 0.4] # [rad^2/sec^2]; from Yun, 2006
if Q_k is None:
# Set the default input, from Yun et al.
Q_k = np.zeros((7, 7)) # process noise matrix Q_k
for ii in range(3):
Q_k[ii,
ii] = D[ii] / (2 * tau[ii]) * (1 -
np.exp(-2 * tstep / tau[ii]))
else:
# Check the shape of the input
assert Q_k.shape == (7, 7)
# Evaluate measurement noise covariance matrix R_k
if R_k is None:
# Set the default input, from Yun et al.
r_angvel = 0.01
# [rad**2/sec**2]; from Yun, 2006
r_quats = 0.0001
# from Yun, 2006
r_ii = np.zeros(7)
for ii in range(3):
r_ii[ii] = r_angvel
for ii in range(4):
r_ii[ii + 3] = r_quats
R_k = np.diag(r_ii)
else:
# Check the shape of the input
assert R_k.shape == (7, 7)
# Calculation of orientation for every time step
qOut = np.zeros((numData, 4))
for ii in range(numData):
accelVec = acc[ii, :]
magVec = mag[ii, :]
angvelVec = omega[ii, :]
z_k_pre = z_k.copy(
) # watch out: by default, Python passes the reference!!
# Evaluate quaternion based on acceleration and magnetic field data
accelVec_n = vector.normalize(accelVec)
magVec_hor = magVec - accelVec_n * (accelVec_n @ magVec)
magVec_n = vector.normalize(magVec_hor)
basisVectors = np.column_stack(
[magVec_n, np.cross(accelVec_n, magVec_n), accelVec_n])
quatRef = quat.q_inv(rotmat.convert(basisVectors, to='quat')).ravel()
# Calculate Kalman Gain
# K_k = P_k * H_k.T * inv(H_k*P_k*H_k.T + R_k)
K_k = P_k @ np.linalg.inv(P_k + R_k)
# Update measurement vector z_k
z_k[:3] = angvelVec
z_k[3:] = quatRef
# Update state vector x_k
x_k += np.array(K_k @ (z_k - z_k_pre)).ravel()
# Evaluate discrete state transition matrix Phi_k
Delta = np.zeros((7, 7))
Delta[3, :] = np.r_[-x_k[4], -x_k[5], -x_k[6], 0, -x_k[0], -x_k[1],
-x_k[2]]
Delta[4, :] = np.r_[x_k[3], -x_k[6], x_k[5], x_k[0], 0, x_k[2],
-x_k[1]]
Delta[5, :] = np.r_[x_k[6], x_k[3], -x_k[4], x_k[1], -x_k[2], 0,
x_k[0]]
Delta[6, :] = np.r_[-x_k[5], x_k[4], x_k[3], x_k[2], x_k[1], -x_k[0],
0]
Delta *= tstep / 2
Phi_k += Delta
# Update error covariance matrix
P_k = (np.eye(7) - K_k) @ P_k
# Projection of state
# 1) quaternions
x_k[3:] += tstep * 0.5 * quat.q_mult(x_k[3:], np.r_[0,
x_k[:3]]).ravel()
x_k[3:] = vector.normalize(x_k[3:])
# 2) angular velocities
x_k[:3] -= tstep * tau * x_k[:3]
qOut[ii, :] = x_k[3:]
# Projection of error covariance matrix
P_k = Phi_k @ P_k @ Phi_k.T + Q_k
# Calculate Position from Orientation
# pos, vel = calc_position(qOut, acc, initialVelocity, initialPosition, timeVector)
# Make the first position the reference position
qOut = quat.q_mult(qOut, quat.q_inv(referenceOrientation))
return qOut
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)
def calc_quat(omega, q0, rate, CStype):
'''
Take an angular velocity (in rad/s), and convert it into the
corresponding orientation quaternion.
Parameters
----------
omega : array, shape (3,) or (N,3)
angular velocity [rad/s].
q0 : array (3,)
vector-part of quaternion (!!)
rate : float
sampling rate (in [Hz])
CStype: string
coordinate_system, space-fixed ("sf") or body_fixed ("bf")
Returns
-------
quats : array, shape (N,4)
unit quaternion vectors.
Notes
-----
1) The output has the same length as the input. As a consequence, the last velocity vector is ignored.
2) For angular velocity with respect to space ("sf"), the orientation is given by
.. math::
q(t) = \\Delta q(t_n) \\circ \\Delta q(t_{n-1}) \\circ ... \\circ \\Delta q(t_2) \\circ \\Delta q(t_1) \\circ q(t_0)
.. math::
\\Delta \\vec{q_i} = \\vec{n(t)}\\sin (\\frac{\\Delta \\phi (t_i)}{2}) = \\frac{\\vec \\omega (t_i)}{\\left| {\\vec \\omega (t_i)} \\right|}\\sin \\left( \\frac{\\left| {\\vec \\omega ({t_i})} \\right|\\Delta t}{2} \\right)
3) For angular velocity with respect to the body ("bf"), the sequence of quaternions is inverted.
4) Take care that you choose a high enough sampling rate!
Examples
--------
>>> v0 = np.r_[0., 0., 100.] * np.pi/180.
>>> omega = np.tile(v0, (1000,1))
>>> rate = 100
>>> out = quat.calc_quat(omega, [0., 0., 0.], rate, 'sf')
array([[ 1. , 0. , 0. , 0. ],
[ 0.99996192, 0. , 0. , 0.00872654],
[ 0.9998477 , 0. , 0. , 0.01745241],
...,
[-0.74895572, 0. , 0. , 0.66262005],
[-0.75470958, 0. , 0. , 0.65605903],
[-0.76040597, 0. , 0. , 0.64944805]])
'''
omega_05 = np.atleast_2d(omega).copy()
# The following is (approximately) the quaternion-equivalent of the trapezoidal integration (cumtrapz)
if omega_05.shape[1] > 1:
omega_05[:-1] = 0.5 * (omega_05[:-1] + omega_05[1:])
omega_t = np.sqrt(np.sum(omega_05**2, 1))
omega_nonZero = omega_t > 0
# initialize the quaternion
q_delta = np.zeros(omega_05.shape)
q_pos = np.zeros((len(omega_05), 4))
q_pos[0, :] = q0
# magnitude of position steps
dq_total = np.sin(omega_t[omega_nonZero] / (2. * rate))
q_delta[omega_nonZero, :] = omega_05[omega_nonZero, :] * np.tile(
dq_total / omega_t[omega_nonZero], (3, 1)).T
for ii in range(len(omega_05) - 1):
q1 = q_delta[ii, :]
q2 = q_pos[ii, :]
if CStype == 'sf':
qm = quat.q_mult(q1, q2)
elif CStype == 'bf':
qm = quat.q_mult(q2, q1)
else:
print('I don' 't know this type of coordinate system!')
q_pos[ii + 1, :] = qm
# print(q_pos)
return q_pos
def kalman(rate, acc, omega, mag):
'''
Calclulate the orientation from IMU magnetometer data.
Parameters
----------
rate : float
sample rate [Hz]
acc : (N,3) ndarray
linear acceleration [m/sec^2]
omega : (N,3) ndarray
angular velocity [rad/sec]
mag : (N,3) ndarray
magnetic field orientation
Returns
-------
qOut : (N,4) ndarray
unit quaternion, describing the orientation relativ to the coordinate system spanned by the local magnetic field, and gravity
Notes
-----
Based on "Design, Implementation, and Experimental Results of a Quaternion-Based Kalman Filter for Human Body Motion Tracking" Yun, X. and Bachman, E.R., IEEE TRANSACTIONS ON ROBOTICS, VOL. 22, 1216-1227 (2006)
'''
numData = len(acc)
# Set parameters for Kalman Filter
tstep = 1. / rate
tau = [0.5, 0.5, 0.5] # from Yun, 2006
# Initializations
x_k = np.zeros(7) # state vector
z_k = np.zeros(7) # measurement vector
z_k_pre = np.zeros(7)
P_k = np.matrix(np.eye(7)) # error covariance matrix P_k
Phi_k = np.matrix(np.zeros(
(7, 7))) # discrete state transition matrix Phi_k
for ii in range(3):
Phi_k[ii, ii] = np.exp(-tstep / tau[ii])
H_k = np.matrix(np.eye(7)) # Identity matrix
Q_k = np.matrix(np.zeros((7, 7))) # process noise matrix Q_k
D = 0.0001 * np.r_[0.4, 0.4, 0.4] # [rad^2/sec^2]; from Yun, 2006
# check 0.0001 in Yun
for ii in range(3):
Q_k[ii,
ii] = D[ii] / (2 * tau[ii]) * (1 - np.exp(-2 * tstep / tau[ii]))
# Evaluate measurement noise covariance matrix R_k
R_k = np.matrix(np.zeros((7, 7)))
r_angvel = 0.01
# [rad**2/sec**2]; from Yun, 2006
r_quats = 0.0001
# from Yun, 2006
for ii in range(7):
if ii < 3:
R_k[ii, ii] = r_angvel
else:
R_k[ii, ii] = r_quats
# Calculation of orientation for every time step
qOut = np.zeros((numData, 4))
for ii in range(numData):
accelVec = acc[ii, :]
magVec = mag[ii, :]
angvelVec = omega[ii, :]
z_k_pre = z_k.copy(
) # watch out: by default, Python passes the reference!!
# Evaluate quaternion based on acceleration and magnetic field data
accelVec_n = vector.normalize(accelVec)
magVec_hor = magVec - accelVec_n * accelVec_n.dot(magVec)
magVec_n = vector.normalize(magVec_hor)
basisVectors = np.vstack((magVec_n, np.cross(accelVec_n,
magVec_n), accelVec_n)).T
quatRef = quat.q_inv(quat.rotmat2quat(basisVectors)).flatten()
# Update measurement vector z_k
z_k[:3] = angvelVec
z_k[3:] = quatRef
# Calculate Kalman Gain
# K_k = P_k * H_k.T * inv(H_k*P_k*H_k.T + R_k)
# Check: why is H_k used in the original formulas?
K_k = P_k * np.linalg.inv(P_k + R_k)
# Update state vector x_k
x_k += np.array(K_k.dot(z_k - z_k_pre)).ravel()
# Evaluate discrete state transition matrix Phi_k
Phi_k[3, :] = np.r_[-x_k[4] * tstep / 2, -x_k[5] * tstep / 2,
-x_k[6] * tstep / 2, 1, -x_k[0] * tstep / 2,
-x_k[1] * tstep / 2, -x_k[2] * tstep / 2]
Phi_k[4, :] = np.r_[x_k[3] * tstep / 2, -x_k[6] * tstep / 2,
x_k[5] * tstep / 2, x_k[0] * tstep / 2, 1,
x_k[2] * tstep / 2, -x_k[1] * tstep / 2]
Phi_k[5, :] = np.r_[x_k[6] * tstep / 2, x_k[3] * tstep / 2,
-x_k[4] * tstep / 2, x_k[1] * tstep / 2,
-x_k[2] * tstep / 2, 1, x_k[0] * tstep / 2]
Phi_k[6, :] = np.r_[-x_k[5] * tstep / 2, x_k[4] * tstep / 2,
x_k[3] * tstep / 2, x_k[2] * tstep / 2,
x_k[1] * tstep / 2, -x_k[0] * tstep / 2, 1]
# Update error covariance matrix
#P_k = (eye(7)-K_k*H_k)*P_k
# Check: why is H_k used in the original formulas?
P_k = (H_k - K_k) * P_k
# Projection of state quaternions
x_k[3:] += quat.q_mult(0.5 * x_k[3:], np.r_[0, x_k[:3]]).flatten()
x_k[3:] = vector.normalize(x_k[3:])
x_k[:3] = np.zeros(3)
x_k[:3] = tstep * (-x_k[:3] + z_k[:3])
qOut[ii, :] = x_k[3:]
# Projection of error covariance matrix
P_k = Phi_k * P_k * Phi_k.T + Q_k
# Make the first position the reference position
qOut = quat.q_mult(qOut, quat.q_inv(qOut[0]))
return qOut
q_data = np.vstack((q_unit, q_non_unit))
print('\nGeneral quaternions:')
pprint(q_data)
# Inversion
q_inverted = quat.q_inv(q_data)
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:')
def analyze_3Dmarkers(MarkerPos, ReferencePos):
'''
Take recorded positions from 3 markers, and calculate center-of-mass (COM) and orientation
Can be used e.g. for the analysis of Optotrac data.
Parameters
----------
MarkerPos : ndarray, shape (N,9)
x/y/z coordinates of 3 markers
ReferencePos : ndarray, shape (1,9)
x/y/z coordinates of markers in the reference position
Returns
-------
Position : ndarray, shape (N,3)
x/y/z coordinates of COM, relative to the reference position
Orientation : ndarray, shape (N,3)
Orientation relative to reference orientation, expressed as quaternion
Example
-------
>>> (PosOut, OrientOut) = analyze_3Dmarkers(MarkerPos, ReferencePos)
'''
# Specify where the x-, y-, and z-position are located in the data
cols = {'x': r_[(0, 3, 6)]}
cols['y'] = cols['x'] + 1
cols['z'] = cols['x'] + 2
# Calculate the position
cog = np.vstack((sum(MarkerPos[:, cols['x']],
axis=1), sum(MarkerPos[:, cols['y']], axis=1),
sum(MarkerPos[:, cols['z']], axis=1))).T / 3.
cog_ref = np.vstack(
(sum(ReferencePos[cols['x']]), sum(
ReferencePos[cols['y']]), sum(ReferencePos[cols['z']]))).T / 3.
position = cog - cog_ref
# Calculate the orientation
numPoints = len(MarkerPos)
orientation = np.ones((numPoints, 3))
refOrientation = vector.plane_orientation(ReferencePos[:3],
ReferencePos[3:6],
ReferencePos[6:])
for ii in range(numPoints):
'''The three points define a triangle. The first rotation is such
that the orientation of the reference-triangle, defined as the
direction perpendicular to the triangle, is rotated along the shortest
path to the current orientation.
In other words, this is a rotation outside the plane spanned by the three
marker points.'''
curOrientation = vector.plane_orientation(MarkerPos[ii, :3],
MarkerPos[ii, 3:6],
MarkerPos[ii, 6:])
alpha = vector.angle(refOrientation, curOrientation)
if alpha > 0:
n1 = vector.normalize(np.cross(refOrientation, curOrientation))
q1 = n1 * np.sin(alpha / 2)
else:
q1 = r_[0, 0, 0]
# Now rotate the triangle into this orientation ...
refPos_after_q1 = vector.rotate_vector(
np.reshape(ReferencePos, (3, 3)), q1)
'''Find which further rotation in the plane spanned by the three marker points
is necessary to bring the data into the measured orientation.'''
Marker_0 = MarkerPos[ii, :3]
Marker_1 = MarkerPos[ii, 3:6]
Vector10 = Marker_0 - Marker_1
vector10_ref = refPos_after_q1[0] - refPos_after_q1[1]
beta = vector.angle(Vector10, vector10_ref)
q2 = curOrientation * np.sin(beta / 2)
if np.cross(vector10_ref, Vector10).dot(curOrientation) <= 0:
q2 = -q2
orientation[ii, :] = quat.q_mult(q2, q1)
return (position, orientation)