How to use the vector.normalize function in vector

To help you get started, we’ve selected a few vector examples, based on popular ways it is used in public projects.

Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.

github thomas-haslwanter / scikit-kinematics / tests / test_vector.py View on Github external
def test_normalize(self):
        result = vector.normalize([3, 0, 0])
        correct = array([[ 1.,  0.,  0.]])
        error = norm(result-correct)
        self.assertAlmostEqual(error, 0)
github thomas-haslwanter / scikit-kinematics / skinematics / imus.py View on Github external
# Update state vector x_k
        x_k += np.array( K_k@(z_k-z_k_pre) ).ravel()

        # Evaluate discrete state transition matrix Phi_k
        Phi_k[3,:] = np.r_[-x_k[4]*tstep/2, -x_k[5]*tstep/2, -x_k[6]*tstep/2,              1, -x_k[0]*tstep/2, -x_k[1]*tstep/2, -x_k[2]*tstep/2]
        Phi_k[4,:] = np.r_[ x_k[3]*tstep/2, -x_k[6]*tstep/2,  x_k[5]*tstep/2, x_k[0]*tstep/2,               1,  x_k[2]*tstep/2, -x_k[1]*tstep/2]
        Phi_k[5,:] = np.r_[ x_k[6]*tstep/2,  x_k[3]*tstep/2, -x_k[4]*tstep/2, x_k[1]*tstep/2, -x_k[2]*tstep/2,               1,  x_k[0]*tstep/2]
        Phi_k[6,:] = np.r_[-x_k[5]*tstep/2,  x_k[4]*tstep/2,  x_k[3]*tstep/2, x_k[2]*tstep/2,  x_k[1]*tstep/2, -x_k[0]*tstep/2,               1]

        # Update error covariance matrix
        P_k = (np.eye(7) - K_k) @ P_k

        # Projection of state
        # 1) quaternions
        x_k[3:] += tstep * 0.5 * quat.q_mult(x_k[3:], np.r_[0, x_k[:3]]).flatten()
        x_k[3:] = vector.normalize( x_k[3:] )
        # 2) angular velocities
        x_k[:3] -= tstep * tau * x_k[:3]

        qOut[ii,:] = x_k[3:]

        # Projection of error covariance matrix
        P_k = Phi_k @ P_k @ Phi_k.T + Q_k

    # Make the first position the reference position
    qOut = quat.q_mult(qOut, quat.q_inv(qOut[0]))

    return qOut
github pypilot / pypilot / pypilot / calibration_fit.py View on Github external
debug('FitLeastSq new_sphere1 failed!!!! ', len(points), new_sphere1d_fit)
        new_sphere1d_fit = current
    else:
        new_sphere1d_fit = lmap(lambda x, a: x + new_sphere1d_fit[0]*a, initial[:3], norm) + [new_sphere1d_fit[1], math.degrees(math.asin(new_sphere1d_fit[2]))]
    new_sphere1d_fit = [new_sphere1d_fit, ComputeDeviation(points, new_sphere1d_fit), 1]
        #print('new sphere1 fit', new_sphere1d_fit)

    if line_max_dev < 2:
        debug('line fit found, insufficient data', line_dev, line_max_dev)
        return False
    
    # 2d sphere fit across normal vector
    u = vector.cross(norm, [norm[1]-norm[2], norm[2]-norm[0], norm[0]-norm[1]])
    v = vector.cross(norm, u)
    u = vector.normalize(u)
    v = vector.normalize(v)

    # initial is the closest to guess on the uv plane containing current
    initial = vector.add(guess[:3], vector.project(vector.sub(current[:3], guess[:3]), norm))
    initial.append(current[3])
    #debug('initial 2d fit', initial)
    
    '''
    def f_sphere2(beta, x):
        bias = lmap(lambda x, a, b: x + beta[0]*a + beta[1]*b, initial[:3], u, v)
        b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
        m = list(numpy.array(b.transpose()))
        r0 = lmap(lambda y : beta[2] - vector.norm(y), m)
        return r0
    sphere2d_fit = FitLeastSq([0, 0, initial[3]], f_sphere2, zpoints)
    if not sphere2d_fit or sphere2d_fit[2] < 0:
        print('FitLeastSq sphere2d failed!!!! ', len(points))
github thomas-haslwanter / scikit-kinematics / skinematics / imus.py View on Github external
# Check the shape of the input
        assert R_k.shape == (7,7)

    # Calculation of orientation for every time step
    qOut = np.zeros( (numData,4) )

    for ii in range(numData):
        accelVec  = acc[ii,:]
        magVec    = mag[ii,:]
        angvelVec = omega[ii,:]
        z_k_pre = z_k.copy()	# watch out: by default, Python passes the reference!!

        # Evaluate quaternion based on acceleration and magnetic field data 
        accelVec_n = vector.normalize(accelVec)
        magVec_hor = magVec - accelVec_n * (accelVec_n @ magVec)
        magVec_n   = vector.normalize(magVec_hor)
        basisVectors = np.vstack( (magVec_n, np.cross(accelVec_n, magVec_n), accelVec_n) ).T
        quatRef = quat.q_inv(rotmat.convert(basisVectors, to='quat')).flatten()

        # Calculate Kalman Gain
        # K_k = P_k * H_k.T * inv(H_k*P_k*H_k.T + R_k)
        K_k = P_k @ np.linalg.inv(P_k + R_k)

        # Update measurement vector z_k
        z_k[:3] = angvelVec
        z_k[3:] = quatRef

        # Update state vector x_k
        x_k += np.array( K_k@(z_k-z_k_pre) ).ravel()

        # Evaluate discrete state transition matrix Phi_k
        Phi_k[3,:] = np.r_[-x_k[4]*tstep/2, -x_k[5]*tstep/2, -x_k[6]*tstep/2,              1, -x_k[0]*tstep/2, -x_k[1]*tstep/2, -x_k[2]*tstep/2]
github pypilot / pypilot / pypilot / calibration_fit.py View on Github external
if not new_sphere1d_fit or new_sphere1d_fit[1] < 0 or abs(new_sphere1d_fit[2]) > 1:
        debug('FitLeastSq new_sphere1 failed!!!! ', len(points), new_sphere1d_fit)
        new_sphere1d_fit = current
    else:
        new_sphere1d_fit = lmap(lambda x, a: x + new_sphere1d_fit[0]*a, initial[:3], norm) + [new_sphere1d_fit[1], math.degrees(math.asin(new_sphere1d_fit[2]))]
    new_sphere1d_fit = [new_sphere1d_fit, ComputeDeviation(points, new_sphere1d_fit), 1]
        #print('new sphere1 fit', new_sphere1d_fit)

    if line_max_dev < 2:
        debug('line fit found, insufficient data', line_dev, line_max_dev)
        return False
    
    # 2d sphere fit across normal vector
    u = vector.cross(norm, [norm[1]-norm[2], norm[2]-norm[0], norm[0]-norm[1]])
    v = vector.cross(norm, u)
    u = vector.normalize(u)
    v = vector.normalize(v)

    # initial is the closest to guess on the uv plane containing current
    initial = vector.add(guess[:3], vector.project(vector.sub(current[:3], guess[:3]), norm))
    initial.append(current[3])
    #debug('initial 2d fit', initial)
    
    '''
    def f_sphere2(beta, x):
        bias = lmap(lambda x, a, b: x + beta[0]*a + beta[1]*b, initial[:3], u, v)
        b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
        m = list(numpy.array(b.transpose()))
        r0 = lmap(lambda y : beta[2] - vector.norm(y), m)
        return r0
    sphere2d_fit = FitLeastSq([0, 0, initial[3]], f_sphere2, zpoints)
    if not sphere2d_fit or sphere2d_fit[2] < 0:
github thomas-haslwanter / scikit-kinematics / skinematics / imus.py View on Github external
R_k[ii,ii] = r_quats
    else:
        # Check the shape of the input
        assert R_k.shape == (7,7)

    # Calculation of orientation for every time step
    qOut = np.zeros( (numData,4) )

    for ii in range(numData):
        accelVec  = acc[ii,:]
        magVec    = mag[ii,:]
        angvelVec = omega[ii,:]
        z_k_pre = z_k.copy()	# watch out: by default, Python passes the reference!!

        # Evaluate quaternion based on acceleration and magnetic field data 
        accelVec_n = vector.normalize(accelVec)
        magVec_hor = magVec - accelVec_n * (accelVec_n @ magVec)
        magVec_n   = vector.normalize(magVec_hor)
        basisVectors = np.vstack( (magVec_n, np.cross(accelVec_n, magVec_n), accelVec_n) ).T
        quatRef = quat.q_inv(rotmat.convert(basisVectors, to='quat')).flatten()

        # Calculate Kalman Gain
        # K_k = P_k * H_k.T * inv(H_k*P_k*H_k.T + R_k)
        K_k = P_k @ np.linalg.inv(P_k + R_k)

        # Update measurement vector z_k
        z_k[:3] = angvelVec
        z_k[3:] = quatRef

        # Update state vector x_k
        x_k += np.array( K_k@(z_k-z_k_pre) ).ravel()