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 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 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 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 _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)