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 _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 testTrajectories(self):
position = vectors.vector(0,0,0)
# Accelerometer test trajectories
for angles in [
(0,-90,0),(0,90,0), # X down, X up (pitch -/+90 deg)
(0,0,90),(0,0,-90), # Y down, Y up (roll +/-90 deg)
(0,0,0),(0,0,180)]: # Z down, Z up (roll 0/180 deg)
rotation = Quaternion().setFromEuler(angles)
yield StaticTrajectory(rotation=rotation)
# Magnetometer test trajectories
field = self.environment.magneticField(position, 0)
N = field / vectors.norm(field)
E = vectors.vector(-N[1,0], N[0,0], 0)
E /= vectors.norm(E)
D = vectors.cross(N, E)
for vectorset in [
(N,E,D), (-N,E,-D), # X north, X south
(E,-N,D), (-E,N,D), # Y north, Y south
(D,E,-N), (-D,E,N)]: # Z north, Z south
rotation = Quaternion().setFromVectors(*vectorset)
yield StaticTrajectory(rotation=rotation)
# Gyro test trajectories
for axis in range(3):
omega = vectors.vector(0,0,0)
omega[axis] = self.rotationalVelocity
yield StaticContinuousRotationTrajectory(omega)
yield StaticContinuousRotationTrajectory(-omega)
def testTrajectories(self):
position = vectors.vector(0, 0, 0)
# Accelerometer test trajectories
for angles in [
(0, -90, 0),
(0, 90, 0), # X down, X up (pitch -/+90 deg)
(0, 0, 90),
(0, 0, -90), # Y down, Y up (roll +/-90 deg)
(0, 0, 0),
(0, 0, 180)
]: # Z down, Z up (roll 0/180 deg)
rotation = Quaternion().setFromEuler(angles)
yield StaticTrajectory(rotation=rotation)
# Magnetometer test trajectories
field = self.environment.magneticField(position, 0)
N = field / vectors.norm(field)
E = vectors.vector(-N[1, 0], N[0, 0], 0)
E /= vectors.norm(E)
D = vectors.cross(N, E)
for vectorset in [
(N, E, D),
(-N, E, -D), # X north, X south
(E, -N, D),
(-E, N, D), # Y north, Y south
(D, E, -N),
(-D, E, N)
]: # Z north, Z south
rotation = Quaternion().setFromVectors(*vectorset)
yield StaticTrajectory(rotation=rotation)
# Gyro test trajectories
for axis in range(3):
omega = vectors.vector(0, 0, 0)
omega[axis] = self.rotationalVelocity
yield StaticContinuousRotationTrajectory(omega)
yield StaticContinuousRotationTrajectory(-omega)
def _process(self, g, m):
"""
Estimate the orientation Quaternion from the observed vectors.
@param g: the observed gravitational field vector (3x1 L{np.ndarray})
@param m: the observed magnetic field vector (3x1 L{np.ndarray})
@return: The estimated orientation L{Quaternion}
"""
z = g / vectors.norm(g)
y = vectors.cross(z, m)
y /= vectors.norm(y)
x = vectors.cross(y, z)
return Quaternion.fromVectors(x, y, z)
def _process(self, g, m):
"""
Estimate the orientation Quaternion from the observed vectors.
@param g: the observed gravitational field vector (3x1 L{np.ndarray})
@param m: the observed magnetic field vector (3x1 L{np.ndarray})
@return: The estimated orientation L{Quaternion}
"""
z = g / vectors.norm(g)
y = vectors.cross(z, m)
y /= vectors.norm(y)
x = vectors.cross(y, z)
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, *meas):
meas = [m / vectors.norm(m) for m in meas]
EPS = 1e-8
# Rotate the frame as necessary to avoid singularities.
# See Shuster 1979 eqs 74-76.
# Note differences in quaternion permutations from Shuster's paper.
# These ones work, why don't the ones given in the paper?
p = self._apply(self.refs, meas)
if abs(p[0]) < EPS:
refs = [r*np.array([[1,-1,-1]]).T for r in self.refs]
p = self._apply(refs, meas)
if abs(p[0]) < EPS:
refs = [r*np.array([[-1,1,-1]]).T for r in self.refs]
p = self._apply(refs, meas)
if abs(p[0]) < EPS:
refs = [r*np.array([[-1,-1,1]]).T for r in self.refs]
p = self._apply(refs, meas)
q = Quaternion(p[3], p[2], -p[1], -p[0])
else:
q = Quaternion(p[2], -p[3], -p[0], p[1])
else:
q = Quaternion(-p[1], p[0], -p[3], p[2])
else:
q = Quaternion(p[0], p[1], p[2], p[3])
q.normalise()
return q
def _process(self, *meas):
meas = [m / vectors.norm(m) for m in meas]
EPS = 1e-8
# Rotate the frame as necessary to avoid singularities.
# See Shuster 1979 eqs 74-76.
# Note differences in quaternion permutations from Shuster's paper.
# These ones work, why don't the ones given in the paper?
p = self._apply(self.refs, meas)
if abs(p[0]) < EPS:
refs = [r*np.array([[1,-1,-1]]).T for r in self.refs]
p = self._apply(refs, meas)
if abs(p[0]) < EPS:
refs = [r*np.array([[-1,1,-1]]).T for r in self.refs]
p = self._apply(refs, meas)
if abs(p[0]) < EPS:
refs = [r*np.array([[-1,-1,1]]).T for r in self.refs]
p = self._apply(refs, meas)
q = Quaternion(p[3], p[2], -p[1], -p[0])
else:
q = Quaternion(p[2], -p[3], -p[0], p[1])
else:
q = Quaternion(-p[1], p[0], -p[3], p[2])
else:
q = Quaternion(p[0], p[1], p[2], p[3])
q.normalise()
return q
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)
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 _process(self, g, m):
# Algorithm assumes that gravity vector is -z in default position
# This is contrary to the other algorithm so we invert
g = -g
g /= vectors.norm(g)
# Pitch
sinTheta = g[0]
cosTheta = math.sqrt(1 - sinTheta**2)
flag = cosTheta <= self.thetaThreshold
if flag:
# If flag is set then theta is close to zero. Rotate coord frame 20
# degrees to reduce errors
g = self.qAlpha.rotateFrame(g)
m = self.qAlpha.rotateFrame(m)
sinTheta = g[0]
cosTheta = math.sqrt(1 - sinTheta**2)
cosHalfTheta = self.cosHalfAngle(cosTheta)
sinHalfTheta = self.sinHalfAngle(cosTheta, sinTheta)
qp = Quaternion(cosHalfTheta, 0, sinHalfTheta, 0)
# Roll
cosPhi = -g[2] / cosTheta
sinPhi = -g[1] / cosTheta
cosHalfPhi = self.cosHalfAngle(cosPhi)
sinHalfPhi = self.sinHalfAngle(cosPhi, sinPhi)
qr = Quaternion(cosHalfPhi, sinHalfPhi, 0, 0)
# Yaw
m = qr.rotateVector(m)
m = qp.rotateVector(m)
Mx = m[0]
My = m[1]
scale = 1 / math.sqrt(Mx**2 + My**2)
Mx *= scale
My *= scale
# From Yun paper
#cosPsi = Mx * self.Nx + My * self.Ny
#sinPsi = Mx * self.Ny - My * self.Nx
# but we know that Ny is 0 so this simplifies to
cosPsi = Mx
sinPsi = -My
cosHalfPsi = self.cosHalfAngle(cosPsi)
sinHalfPsi = self.sinHalfAngle(cosPsi, sinPsi)
qy = Quaternion(cosHalfPsi, 0, 0, sinHalfPsi)
combined = qy * qp * qr
if flag:
return combined * self.qAlpha.conjugate
else:
return combined
def _process(self, g, m):
# Algorithm assumes that gravity vector is -z in default position
# This is contrary to the other algorithm so we invert
g = -g
g /= vectors.norm(g)
# Pitch
sinTheta = g[0]
cosTheta = math.sqrt(1 - sinTheta**2)
flag = cosTheta <= self.thetaThreshold
if flag:
# If flag is set then theta is close to zero. Rotate coord frame 20
# degrees to reduce errors
g = self.qAlpha.rotateFrame(g)
m = self.qAlpha.rotateFrame(m)
sinTheta = g[0]
cosTheta = math.sqrt(1 - sinTheta**2)
cosHalfTheta = self.cosHalfAngle(cosTheta)
sinHalfTheta = self.sinHalfAngle(cosTheta, sinTheta)
qp = Quaternion(cosHalfTheta, 0, sinHalfTheta, 0)
# Roll
cosPhi = -g[2] / cosTheta
sinPhi = -g[1] / cosTheta
cosHalfPhi = self.cosHalfAngle(cosPhi)
sinHalfPhi = self.sinHalfAngle(cosPhi, sinPhi)
qr = Quaternion(cosHalfPhi, sinHalfPhi, 0, 0)
# Yaw
m = qr.rotateVector(m)
m = qp.rotateVector(m)
Mx = m[0]
My = m[1]
scale = 1 / math.sqrt(Mx**2 + My**2)
Mx *= scale
My *= scale
# From Yun paper
#cosPsi = Mx * self.Nx + My * self.Ny
#sinPsi = Mx * self.Ny - My * self.Nx
# but we know that Ny is 0 so this simplifies to
cosPsi = Mx
sinPsi = -My
cosHalfPsi = self.cosHalfAngle(cosPsi)
sinHalfPsi = self.sinHalfAngle(cosPsi, sinPsi)
qy = Quaternion(cosHalfPsi, 0, 0, sinHalfPsi)
combined = qy * qp * qr
if flag:
return combined * self.qAlpha.conjugate
else:
return combined
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 _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 _update(self, accel, mag, gyro, dt, t):
dotq = 0.5 * self.qHat * Quaternion(0,*gyro)
self.qHat += dotq * dt
if abs(vectors.norm(accel) - 1) < self._aT:
qMeas = self._vectorObservation(-accel, mag)
if self.qHat.dot(qMeas) < 0:
qMeas.negate()
qError = qMeas - self.qHat
self.qHat += (1/self._k) * dt * qError
else:
qMeas = Quaternion.nan()
self.vectorObservation.add(t, qMeas)
self.qHat.normalise()
self.rotation.add(t, self.qHat)
def _update(self, accel, mag, gyro, dt, t):
dotq = 0.5 * self.qHat * Quaternion(0, *gyro)
self.qHat += dotq * dt
if abs(vectors.norm(accel) - 1) < self._aT:
qMeas = self._vectorObservation(-accel, mag)
if self.qHat.dot(qMeas) < 0:
qMeas.negate()
qError = qMeas - self.qHat
self.qHat += (1 / self._k) * dt * qError
else:
qMeas = Quaternion.nan()
self.vectorObservation.add(t, qMeas)
self.qHat.normalise()
self.rotation.add(t, self.qHat)
def nominalMagnitude(self):
"""
Nominal magnitude of the field, for use in calibrating sensors.
"""
return vectors.norm(self.nominalValue)
def nominalMagnitude(self):
"""
Nominal magnitude of the field, for use in calibrating sensors.
"""
return vectors.norm(self.nominalValue)
def _generateDataPoints(self, t, endEffector):
return np.hstack(j.position(t) for j in endEffector.ascendTree()
if j.parent is None or vectors.norm(j.positionOffset) > 0)
def testNorm():
for v in basisVectors:
assert_almost_equal(vectors.norm(v), 1)
def _generateDataPoints(self, t, endEffector):
return np.hstack(
j.position(t) for j in endEffector.ascendTree()
if j.parent is None or vectors.norm(j.positionOffset) > 0)
def error(p):
cal = ScaleAndOffsetCalibration(*params(p))
result = cal.apply(measured)
return vectors.norm(result - expected)
def testNorm():
for v in basisVectors:
assert_almost_equal(vectors.norm(v), 1)
def error(p):
cal = ScaleAndOffsetCalibration(*params(p))
result = cal.apply(measured)
return vectors.norm(result - expected)
