Esempio n. 1
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)
Esempio n. 2
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)
Esempio n. 3
0
 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)
Esempio n. 4
0
 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)
Esempio n. 5
0
    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)
Esempio n. 6
0
    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)
Esempio n. 7
0
    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()
Esempio n. 8
0
    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()
Esempio n. 9
0
    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
Esempio n. 10
0
    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
Esempio n. 11
0
    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)
Esempio n. 12
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])
Esempio n. 13
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
Esempio n. 14
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
Esempio n. 15
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
Esempio n. 16
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
Esempio n. 18
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])
Esempio n. 19
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)
Esempio n. 20
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)
Esempio n. 21
0
 def nominalMagnitude(self):
     """
     Nominal magnitude of the field, for use in calibrating sensors.
     """
     return vectors.norm(self.nominalValue)
Esempio n. 22
0
 def nominalMagnitude(self):
     """
     Nominal magnitude of the field, for use in calibrating sensors.
     """
     return vectors.norm(self.nominalValue)
Esempio n. 23
0
 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)
Esempio n. 24
0
def testNorm():
    for v in basisVectors:
        assert_almost_equal(vectors.norm(v), 1)
Esempio n. 25
0
 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)
Esempio n. 26
0
 def error(p):
     cal = ScaleAndOffsetCalibration(*params(p))
     result = cal.apply(measured)
     return vectors.norm(result - expected)
Esempio n. 27
0
def testNorm():
    for v in basisVectors:
        assert_almost_equal(vectors.norm(v), 1)
Esempio n. 28
0
 def error(p):
     cal = ScaleAndOffsetCalibration(*params(p))
     result = cal.apply(measured)
     return vectors.norm(result - expected)