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 _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, *meas):
"""
Estimate the orientation Quaternion from the observed vectors.
@param meas: The observed vectors (3x1 L{np.ndarray})
@return: The least squares optimal L{Quaternion}
"""
B = sum([
a * np.dot(b, r.T)
for a, b, r in zip(self.weights, meas, self.refs)
])
S = B + B.T
z = sum([
a * vectors.cross(b, r)
for a, b, r in zip(self.weights, meas, self.refs)
])
sigma = np.trace(B)
K = np.empty((4, 4))
K[0:3, 0:3] = S - sigma * np.identity(3)
K[0:3, 3] = z[:, 0]
K[3, 0:3] = z[:, 0]
K[3, 3] = sigma
eigenValues, eigenVectors = np.linalg.eig(K)
q = np.asarray(eigenVectors[:, np.argmax(eigenValues)])
return Quaternion(q[3], q[0], q[1], q[2]).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 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 velocity(self, t):
v = self.parent.velocity(t)
r = self.parent.rotation(t)
o = r.rotateVector(self.positionOffset)
omega = self.parent.rotationalVelocity(t)
rv = vectors.cross(omega, o)
return v + rv
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 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):
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 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 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 _process(self, *meas):
"""
Estimate the orientation Quaternion from the observed vectors.
@param meas: The observed vectors (3x1 L{np.ndarray})
@return: The least squares optimal L{Quaternion}
"""
B = sum([a * np.dot(b, r.T)
for a,b,r in zip(self.weights, meas, self.refs)])
S = B + B.T
z = sum([a * vectors.cross(b, r)
for a,b,r in zip(self.weights, meas, self.refs)])
sigma = np.trace(B)
K = np.empty((4,4))
K[0:3,0:3] = S-sigma*np.identity(3)
K[0:3,3] = z[:,0]
K[3,0:3] = z[:,0]
K[3,3] = sigma
eigenValues,eigenVectors = np.linalg.eig(K)
q = np.asarray(eigenVectors[:,np.argmax(eigenValues)])
return Quaternion(q[3],q[0],q[1],q[2]).normalise()
def testCross():
assert_almost_equal(vectors.cross(e1, e2), e3)
assert_almost_equal(vectors.cross(e1, e3), -e2)
assert_almost_equal(vectors.cross(e2, e3), e1)
def testCross():
assert_almost_equal(vectors.cross(e1, e2), e3)
assert_almost_equal(vectors.cross(e1, e3), -e2)
assert_almost_equal(vectors.cross(e2, e3), e1)
