def testDot():
for v1 in basisVectors:
for v2 in basisVectors:
if v1 is v2:
assert_almost_equal(vectors.dot(v1, v2), 1)
else:
assert_almost_equal(vectors.dot(v1, v2), 0)
def testDot():
for v1 in basisVectors:
for v2 in basisVectors:
if v1 is v2:
assert_almost_equal(vectors.dot(v1, v2), 1)
else:
assert_almost_equal(vectors.dot(v1, v2), 0)
def _process(self, g, m):
z = g / vectors.norm(g)
x = m - (z * vectors.dot(m, z))
x /= vectors.norm(x)
y = vectors.cross(z, x)
return Quaternion.fromVectors(x, y, z)
def handleLinearAcceleration(self, jointAcceleration, dt):
"""
Perform drift correction based on incoming joint acceleration estimate.
@param jointAcceleration: Acceleration estimate (3x1 L{np.ndarray}).
"""
self.jointAccel = jointAcceleration
if len(self.gyroFIFO) < 3:
return
# Estimate linear acceleration
o = self.offset
w = self.gyroFIFO
q = self.qHat_minus_1
a = (w[0] - w[2]) / (2 * dt)
lt = vectors.cross(a, o)
lr = vectors.dot(w[1], o) * w[1] - o * vectors.norm(w[1])**2
l = (q.rotateFrame(self.jointAccel) + lt + lr)
# Apply drift correction
self.qMeas = self._vectorObservation(-(self.accelFIFO[1] - l),
self.magFIFO[1])
if q.dot(self.qMeas) < 0:
self.qMeas.negate()
q = q + 1.0 / self.k * dt * (self.qMeas - q)
dotq = 0.5 * self.qHat * Quaternion(0, *w[0])
self.qHat = q + dotq * dt
self.qHat.normalise()
def handleLinearAcceleration(self, jointAcceleration, dt):
"""
Perform drift correction based on incoming joint acceleration estimate.
@param jointAcceleration: Acceleration estimate (3x1 L{np.ndarray}).
"""
self.jointAccel = jointAcceleration
if len(self.gyroFIFO) < 3:
return
# Estimate linear acceleration
o = self.offset
w = self.gyroFIFO
q = self.qHat_minus_1
a = (w[0] - w[2]) / (2 * dt)
lt = vectors.cross(a, o)
lr = vectors.dot(w[1], o) * w[1] - o * vectors.norm(w[1])**2
l = (q.rotateFrame(self.jointAccel) + lt + lr)
# Apply drift correction
self.qMeas = self._vectorObservation(-(self.accelFIFO[1]- l),
self.magFIFO[1])
if q.dot(self.qMeas) < 0:
self.qMeas.negate()
q = q + 1.0/self.k * dt * (self.qMeas - q)
dotq = 0.5 * self.qHat * Quaternion(0,*w[0])
self.qHat = q + dotq * dt
self.qHat.normalise()
def _process(self, g, m):
z = g / vectors.norm(g)
x = m - (z * vectors.dot(m, z))
x /= vectors.norm(x)
y = vectors.cross(z, x)
return Quaternion.fromVectors(x, y, z)
def _apply(self, refs, meas):
B = sum(
[a * np.dot(m, r.T) for a, m, r in zip(self.weights, meas, refs)])
S = B + B.T
Z = sum([
a * vectors.cross(m, r)
for a, m, r in zip(self.weights, meas, refs)
])
sigma = np.trace(B)
kappa = ((S[1, 1] * S[2, 2] - S[1, 2] * S[2, 1]) +
(S[0, 0] * S[2, 2] - S[0, 2] * S[2, 0]) +
(S[0, 0] * S[1, 1] - S[0, 1] * S[1, 0]))
# where did the above come from and why doesn't this work instead?:
# kappa = np.trace(np.matrix(S).H)
delta = np.linalg.det(S)
if len(refs) == 2:
# Closed form solution for lambdaMax, see Shuster 1979 eqs 72-3.
cosTerm = vectors.dot(*refs)[0]*vectors.dot(*meas)[0] + \
vectors.norm(vectors.cross(*refs))*vectors.norm(vectors.cross(*meas))
lambdaMax = math.sqrt(
np.sum(self.weights**2) +
2 * np.product(self.weights) * cosTerm)
else:
# Iterate for lambdaMax, using substitutions from Shuster JAS1290 paper
# for better numerical accuracy.
a = sigma**2 - kappa
b = sigma**2 + np.dot(Z.T, Z)[0, 0]
c = 8 * np.linalg.det(B)
d = np.dot(Z.T, np.dot(np.dot(S, S), Z))[0, 0]
f = lambda l: (l**2 - a) * (l**2 - b) - c * l + c * sigma - d
fprime = lambda l: 4 * l**3 - 2 * (a + b) * l + c
lambdaMax = newton(f, 1.0, fprime)
alpha = lambdaMax**2 - sigma**2 + kappa
beta = lambdaMax - sigma
gamma = (lambdaMax + sigma) * alpha - delta
X = np.dot(alpha * np.identity(3) + beta * S + np.dot(S, S), Z)
return [gamma] + list(X[0:3, 0])
def _apply(self, refs, meas):
B = sum([a * np.dot(m, r.T) for a,m,r in zip(self.weights, meas, refs)])
S = B + B.T
Z = sum([a * vectors.cross(m, r) for a,m,r in zip(self.weights, meas, refs)])
sigma = np.trace(B)
kappa = ((S[1,1]*S[2,2] - S[1,2]*S[2,1]) +
(S[0,0]*S[2,2] - S[0,2]*S[2,0]) +
(S[0,0]*S[1,1] - S[0,1]*S[1,0]))
# where did the above come from and why doesn't this work instead?:
# kappa = np.trace(np.matrix(S).H)
delta = np.linalg.det(S)
if len(refs) == 2:
# Closed form solution for lambdaMax, see Shuster 1979 eqs 72-3.
cosTerm = vectors.dot(*refs)[0]*vectors.dot(*meas)[0] + \
vectors.norm(vectors.cross(*refs))*vectors.norm(vectors.cross(*meas))
lambdaMax = math.sqrt(np.sum(self.weights**2) + 2*np.product(self.weights)*cosTerm)
else:
# Iterate for lambdaMax, using substitutions from Shuster JAS1290 paper
# for better numerical accuracy.
a = sigma**2 - kappa
b = sigma**2 + np.dot(Z.T, Z)[0,0]
c = 8 * np.linalg.det(B)
d = np.dot(Z.T, np.dot(np.dot(S,S), Z))[0,0]
f = lambda l: (l**2 - a)*(l**2 - b) - c*l + c*sigma - d
fprime = lambda l: 4*l**3 -2*(a+b)*l + c
lambdaMax = newton(f, 1.0, fprime)
alpha = lambdaMax**2 - sigma**2 + kappa
beta = lambdaMax - sigma
gamma = (lambdaMax + sigma)*alpha - delta
X = np.dot(alpha*np.identity(3) + beta*S + np.dot(S,S), Z)
return [gamma] + list(X[0:3,0])
def childAcceleration(self, o, dt):
"""
Compute acceleration for child joint.
@param o: Offset of child joint (3x1 L{np.ndarray}).
"""
w = self.gyroFIFO
if len(w) < 3:
return None
q = self.qHat_minus_1
a = (w[0] - w[2]) / (2 * dt)
lt = vectors.cross(a, o)
lr = vectors.dot(w[1], o) * w[1] - o * vectors.norm(w[1])**2
l = self.jointAccel + q.rotateVector(lt + lr)
return l
def childAcceleration(self, o, dt):
"""
Compute acceleration for child joint.
@param o: Offset of child joint (3x1 L{np.ndarray}).
"""
w = self.gyroFIFO
if len(w) < 3:
return None
q = self.qHat_minus_1
a = (w[0] - w[2]) / (2 * dt)
lt = vectors.cross(a, o)
lr = vectors.dot(w[1], o) * w[1] - o * vectors.norm(w[1])**2
l = self.jointAccel + q.rotateVector(lt + lr)
return l
def acceleration(self, t):
"""
Uses the algorithm from Young et al. 2010 'Distributed Estimation
of Linear Acceleration for Improved Accuracy in Wireless Inertial
Motion Capture' to calculate linear acceleration from rotational
velocity and acceleration
"""
a = self.parent.acceleration(t)
r = self.parent.rotation(t)
o = r.rotateVector(self.positionOffset)
omega = self.parent.rotationalVelocity(t)
alpha = self.parent.rotationalAcceleration(t)
lt = vectors.cross(alpha, o)
lr = vectors.dot(o, omega) * omega - o * vectors.norm(omega)**2
return a + lt + lr
def headingVariation(self, position, t):
"""
Calculate heading variation due to magnetic distortion.
@param position: 3xN L{np.ndarray} of position(s).
@return: Heading variation(s) in degrees at given positions.
"""
Bx,By,Bz = self(position, t)
hField = np.vstack((Bx.ravel(),By.ravel()))
hField /= vectors.norm(hField)
ref = np.empty_like(hField)
ref[:] = self.nominalValue[:2].copy()
ref /= vectors.norm(ref)
variation = np.arccos(vectors.dot(hField,ref))
return np.degrees(variation)
