Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def test_normalize(self):
result = vector.normalize([3, 0, 0])
correct = array([[ 1., 0., 0.]])
error = norm(result-correct)
self.assertAlmostEqual(error, 0)
# 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
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))
# 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]
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:
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()