示例#1
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)
示例#2
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)
示例#3
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)
示例#4
0
    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()
示例#5
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()
示例#6
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)
 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
示例#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()
示例#9
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)
示例#10
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)
示例#11
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])
示例#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])
示例#13
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
示例#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
示例#16
0
    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()
示例#17
0
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)
示例#18
0
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)