def acc_model(states):
g = np.array([[0, 0, 0, 9.8]])
inv_states = copy.deepcopy(states[:, :4])
inv_states[:, 1:4] = -inv_states[:, 1:4]
g_prime = quaternion.multiply(quaternion.multiply(inv_states, g),
states[:, :4])
return g_prime[:, 1:4]
def qmean(Y, Y_qmean):
iter = 0
while (1):
iter += 1
Y_qmean_inv = np.hstack((Y_qmean[0], -Y_qmean[1:]))
ev_i = quaternion.toVec(
quaternion.multiply(Y[:, :4], Y_qmean_inv[np.newaxis, :]))
e_quat = quaternion.from_rv(np.mean(ev_i, axis=0)[np.newaxis, :])
Y_qmean = quaternion.multiply(e_quat, Y_qmean[np.newaxis, :]).squeeze()
if np.linalg.norm(np.mean(ev_i, axis=0)) < 0.001 or iter > 10000:
break
return Y_qmean, ev_i
def update_alignment(self, q): a2 = 2*math.atan2(q[3], q[0]) heading_offset = a2*180/math.pi off = self.heading_off.value - heading_offset o = quaternion.angvec2quat(off*math.pi/180, [0, 0, 1]) self.alignmentQ.update(quaternion.normalize(quaternion.multiply(q, o))) self.auto_cal.SetNorm(quaternion.rotvecquat([0, 0, 1], self.alignmentQ.value))
def outerloop(p, pdot, psi, ref):
psi_ref = math.radians(ref[3])
p_ref = ref[0:3]
# position control
F_no_g = -np.multiply(Kp_pos, (p - p_ref)) - np.multiply(Kd_pos, pdot)
F = F_no_g - np.matrix([[0, 0, MASS * G]]).T
Tc = -F_no_g[2, 0] # thrust (or vertical speed)
Fxy_norm = np.linalg.norm(F[0:2, 0])
alpha = math.atan2(Fxy_norm, -F[2, 0])
qw = math.cos(alpha / 2)
if Fxy_norm == 0:
qxyz = np.zeros(3)
else:
qxyz = (math.sin(alpha / 2) / Fxy_norm) * np.array(
[F[1, 0], -F[0, 0], 0])
qalpha = np.concatenate([[qw], qxyz])
qpsi = np.array([math.cos(psi / 2), 0, 0, math.sin(psi / 2)])
qc = quat.multiply(qalpha, qpsi)
_, thetac, phic = quat.q_to_euler(qc) # pitch, roll (rad)
# yaw control (rad/s)
delta_psi = psi - psi_ref
if delta_psi < -math.pi:
delta_psi += 2 * math.pi
elif delta_psi > math.pi:
delta_psi -= 2 * math.pi
psic = -Kp_psi * delta_psi
return (Tc, phic, thetac, psic)
def state_update(K, Z_mean, Y_qmean, Y_mean, measurement):
v = measurement - Z_mean
kv = np.matmul(K, v)
q = quaternion.multiply(Y_qmean[np.newaxis, :],
quaternion.from_rv(kv[np.newaxis, :3], 1))
w = Y_mean[4:] + kv[3:]
return np.hstack((q.flatten(), w))
def test_multiply():
# pmath.multiply(q0.numpy(), q1.numpy())
normalized = tq.normalize(torch.Tensor(myq))
normalized2 = tq.normalize(rand_q(8))
multiplied = tq.multiply(normalized, normalized2)
for d in range(normalized.shape[0]):
mul0 = tq.multiply(normalized[d], normalized2[d])
assert torch.all(multiplied[d] == mul0)
for d in range(normalized.shape[0]):
mul0 = pmath.quaternion_multiply(normalized[d].numpy(),
normalized2[d].numpy())
assert np.all(multiplied[d].numpy() == mul0)
def fuse(self, mag_normal, acc):
acc_len = quaternion.vector_len(acc)
if acc_len < 0.95 or acc_len > 1.05:
self.quality = 0.0
return
acc_normal = quaternion.normal(acc)
mag_rot_q = quaternion.from_two_vector(self.mag_vector, mag_normal)
gravity_vector = self.neg_v(acc_normal)
origin_gravity = quaternion.rotate_vector([0.0, 0.0, -1.0], mag_rot_q)
v1 = quaternion.cross(mag_normal, gravity_vector)
v2 = quaternion.cross(mag_normal, origin_gravity)
v1 = quaternion.normal(v1)
v2 = quaternion.normal(v2)
ang_rot_dot = quaternion.dot(v1, v2)
ang_rot = math.acos(ang_rot_dot) * 180.0 / math.pi
v_axis = quaternion.cross(v2, v1)
v_axis = quaternion.normal(v_axis)
ang_axis_dot = quaternion.dot(v_axis, mag_normal)
if ang_axis_dot <= -1.0:
ang_axis_dot = -0.99999
if ang_axis_dot >= 1.0:
ang_axis_dot = 0.99999
ang_axis = math.acos(ang_axis_dot) * 180.0 / math.pi
rot_axis = mag_normal
if ang_axis > 90.0:
rot_axis = self.neg_v(mag_normal)
rot_q = quaternion.from_axis_angle(rot_axis, ang_rot)
rot_gravity = quaternion.rotate_vector(origin_gravity, rot_q)
dot = quaternion.dot(rot_gravity, gravity_vector)
ang = math.acos(dot) * 180.0 / math.pi
quality = ang / 10.0
if quality > 1.0:
quality = 1.0
self.quality = quality
#print '%.2f %.2f %.2f %.2f' % (ang2, ang_axis, ang_rot, ang)
q = quaternion.multiply(rot_q, mag_rot_q)
self.q = self.neg_q(q)
def sigma_points(P, Q, state, dt):
W = np.vstack((np.sqrt(2 * 6) * np.linalg.cholesky(
(P + Q)).transpose(), -np.sqrt(2 * 6) * (np.linalg.cholesky(
(P + Q)).transpose())))
X = np.zeros((7, 12))
X[:4, :] = quaternion.multiply(state[np.newaxis, :4],
quaternion.from_rv(W[:, :3])).transpose()
X[4:, :] = (state[4:] + W[:, 3:]).T
X = X.T
Y = process_model(X, dt)
Z = sensor_model(X)
return Y, Z
def fuse(self, gyro_q, mag_normal, acc):
self.mag_acc_fusion_obj.fuse(mag_normal, acc)
if self.q is None:
self.gyro_init_q = gyro_q
self.adjust_q = self.mag_acc_fusion_obj.q
self.gyro_diff_q = quaternion.diff(self.gyro_init_q, gyro_q)
new_q = quaternion.multiply(self.adjust_q, self.gyro_diff_q)
diff_q = quaternion.diff(new_q, self.mag_acc_fusion_obj.q)
diff_q = quaternion.standardize(diff_q)
step_adj_q = quaternion.scale(diff_q,
self.mag_acc_fusion_obj.quality / 20.0)
self.adjust_q = quaternion.multiply(step_adj_q, self.adjust_q)
self.q = quaternion.multiply(self.adjust_q, self.gyro_diff_q)
control.control(self.q)
def rotate(self):
dx = self.mouse_x - self.mouse_down_x
dy = self.mouse_y - self.mouse_down_y
angle_x = -dx / 10.0
angle_y = -dy / 10.0
right_axis = quaternion.cross(self.up_axis, self.snap_view_point)
qx = quaternion.from_axis_angle(self.up_axis, angle_x)
qy = quaternion.from_axis_angle(right_axis, angle_y)
q = quaternion.multiply(qx, qy)
self.view_point = quaternion.rotate_vector(self.snap_view_point, q)
def IMURead(self):
data = False
while self.poller.poll(0): # read all the data from the pipe
data = self.imu_pipe.recv()
if not data:
if time.time() - self.last_imuread > 1 and self.loopfreq.value:
print 'IMURead failed!'
self.loopfreq.set(0)
for name in self.SensorValues:
self.SensorValues[name].set(False)
self.uptime.reset()
return False
if vector.norm(data['accel']) == 0:
print 'vector n', data['accel']
return False
self.last_imuread = time.time()
self.loopfreq.strobe()
if not self.FirstTimeStamp:
self.FirstTimeStamp = data['timestamp']
data['timestamp'] -= self.FirstTimeStamp
#data['accel_comp'] = quaternion.rotvecquat(vector.sub(data['accel'], down), self.alignmentQ.value)
# apply alignment calibration
gyro_q = quaternion.rotvecquat(data['gyro'], data['fusionQPose'])
data['pitchrate'], data['rollrate'], data['headingrate'] = map(
math.degrees, gyro_q)
origfusionQPose = data['fusionQPose']
aligned = quaternion.multiply(data['fusionQPose'],
self.alignmentQ.value)
data['fusionQPose'] = quaternion.normalize(
aligned) # floating point precision errors
data['roll'], data['pitch'], data['heading'] = map(
math.degrees, quaternion.toeuler(data['fusionQPose']))
if data['heading'] < 0:
data['heading'] += 360
dt = data['timestamp'] - self.lasttimestamp
self.lasttimestamp = data['timestamp']
if dt > .02 and dt < .5:
data['headingraterate'] = (data['headingrate'] -
self.headingrate) / dt
else:
data['headingraterate'] = 0
self.headingrate = data['headingrate']
data['heel'] = self.heel = data['roll'] * .03 + self.heel * .97
#data['roll'] -= data['heel']
data['gyro'] = list(map(math.degrees, data['gyro']))
data['gyrobias'] = list(map(math.degrees, data['gyrobias']))
# lowpass heading and rate
llp = self.heading_lowpass_constant.value
data['heading_lowpass'] = heading_filter(
llp, data['heading'], self.SensorValues['heading_lowpass'].value)
llp = self.headingrate_lowpass_constant.value
data['headingrate_lowpass'] = llp * data['headingrate'] + (
1 - llp) * self.SensorValues['headingrate_lowpass'].value
llp = self.headingraterate_lowpass_constant.value
data['headingraterate_lowpass'] = llp * data['headingraterate'] + (
1 - llp) * self.SensorValues['headingraterate_lowpass'].value
# set sensors
self.server.TimeStamp('imu', data['timestamp'])
for name in self.SensorValues:
self.SensorValues[name].set(data[name])
compass, accel, down = False, False, False
if not self.accel_calibration.locked.value:
accel = list(data['accel'])
if not self.compass_calibration.locked.value:
down = quaternion.rotvecquat([0, 0, 1],
quaternion.conjugate(origfusionQPose))
compass = list(data['compass']) + down
if accel or compass:
self.auto_cal.AddPoint((accel, compass, down))
self.uptime.update()
# count down to alignment
if self.alignmentCounter.value != self.last_alignmentCounter:
self.alignmentPose = [0, 0, 0, 0]
if self.alignmentCounter.value > 0:
self.alignmentPose = list(
map(lambda x, y: x + y, self.alignmentPose,
data['fusionQPose']))
self.alignmentCounter.set(self.alignmentCounter.value - 1)
if self.alignmentCounter.value == 0:
self.alignmentPose = quaternion.normalize(self.alignmentPose)
adown = quaternion.rotvecquat([0, 0, 1],
quaternion.conjugate(
self.alignmentPose))
alignment = []
alignment = quaternion.vec2vec2quat([0, 0, 1], adown)
alignment = quaternion.multiply(self.alignmentQ.value,
alignment)
if len(alignment):
self.update_alignment(alignment)
self.last_alignmentCounter = self.alignmentCounter.value
if self.heading_off.value != self.last_heading_off:
self.update_alignment(self.alignmentQ.value)
self.last_heading_off = self.heading_off.value
result = self.auto_cal.UpdatedCalibration()
if result:
if result[0] == 'accel' and not self.accel_calibration.locked.value:
#print '[boatimu] cal result', result[0]
self.accel_calibration.sigmapoints.set(result[2])
if result[1]:
self.accel_calibration.age.reset()
self.accel_calibration.set(result[1])
elif result[
0] == 'compass' and not self.compass_calibration.locked.value:
#print '[boatimu] cal result', result[0]
self.compass_calibration.sigmapoints.set(result[2])
if result[1]:
self.compass_calibration.age.reset()
self.compass_calibration.set(result[1])
self.accel_calibration.age.update()
self.compass_calibration.age.update()
return data
def fuse(self, mag, gyro_q):
self.take_snap(mag, gyro_q)
self.count += 1
self.increace_q(gyro_q)
self.filter_q()
if self.need_calc == False:
return
q_mag = quaternion.from_two_vector(mag, self.mag_vector)
gyro_diff_neg = self.neg_q(self.gyro_diff)
es = []
for i in range(360):
q_rot = quaternion.from_axis_angle(self.mag_vector, i)
q_mag_rot = quaternion.multiply(q_rot, q_mag)
q_last = quaternion.multiply(gyro_diff_neg, q_mag_rot)
x_axis = quaternion.rotate_vector([1.0, 0.0, 0.0], q_last)
y_axis = quaternion.rotate_vector([0.0, 1.0, 0.0], q_last)
z_axis = quaternion.rotate_vector([0.0, 0.0, 1.0], q_last)
x_v = quaternion.dot(x_axis, self.mag_vector)
y_v = quaternion.dot(y_axis, self.mag_vector)
z_v = quaternion.dot(z_axis, self.mag_vector)
e = 0.0
e += abs(x_v - self.snap_mag[0])
e += abs(y_v - self.snap_mag[1])
e += abs(z_v - self.snap_mag[2])
es.append(e)
min_e = min(es)
min_index = es.index(min_e)
average = sum(es) / len(es)
self.es_list.append(es)
if len(self.es_list) > 30:
#f = open('es_list', 'wb')
#pickle.dump(self.es_list, f)
print 'dumped'
axis, angle = quaternion.get_axis_angle(self.gyro_diff)
axis = quaternion.normal(axis)
dot = quaternion.dot(axis, self.mag_vector)
q_rot = quaternion.from_axis_angle(self.mag_vector, min_index)
q_mag_rot = quaternion.multiply(q_rot, q_mag)
if min_e < 0.03 and dot < 0.9:
print 'updated'
self.snap_q = gyro_q
self.calced_q = q_mag_rot
else:
print 'not acurate'
self.need_calc = False
self.snap_mag = None
def increace_q(self, gyro_q):
q_diff = quaternion.diff(self.snap_q, gyro_q)
self.increaced_q = quaternion.multiply(q_diff, self.calced_q)
def read(self):
data = self.IMUread()
if not data:
if time.monotonic(
) - self.last_imuread > 1 and self.frequency.value:
print('IMURead failed!')
self.frequency.set(False)
for name in self.SensorValues:
self.SensorValues[name].set(False)
self.uptime.reset()
return False
if vector.norm(data['accel']) == 0:
print('accel values invalid', data['accel'])
return False
self.last_imuread = time.monotonic()
self.frequency.strobe()
# apply alignment calibration
gyro_q = quaternion.rotvecquat(data['gyro'], data['fusionQPose'])
data['pitchrate'], data['rollrate'], data['headingrate'] = map(
math.degrees, gyro_q)
aligned = quaternion.multiply(data['fusionQPose'],
self.alignmentQ.value)
aligned = quaternion.normalize(
aligned) # floating point precision errors
data['roll'], data['pitch'], data['heading'] = map(
math.degrees, quaternion.toeuler(aligned))
if data['heading'] < 0:
data['heading'] += 360
dt = data['timestamp'] - self.lasttimestamp
self.lasttimestamp = data['timestamp']
if dt > .01 and dt < .2:
data['headingraterate'] = (data['headingrate'] -
self.headingrate) / dt
else:
data['headingraterate'] = 0
self.headingrate = data['headingrate']
data['heel'] = self.heel = data['roll'] * .03 + self.heel * .97
#data['roll'] -= data['heel']
data['gyro'] = list(map(math.degrees, data['gyro']))
# lowpass heading and rate
llp = self.heading_lowpass_constant.value
data['heading_lowpass'] = heading_filter(
llp, data['heading'], self.SensorValues['heading_lowpass'].value)
llp = self.headingrate_lowpass_constant.value
data['headingrate_lowpass'] = llp * data['headingrate'] + (
1 - llp) * self.SensorValues['headingrate_lowpass'].value
llp = self.headingraterate_lowpass_constant.value
data['headingraterate_lowpass'] = llp * data['headingraterate'] + (
1 - llp) * self.SensorValues['headingraterate_lowpass'].value
# set sensors
for name in self.SensorValues:
self.SensorValues[name].set(data[name])
self.uptime.update()
# count down to alignment
if self.alignmentCounter.value != self.last_alignmentCounter:
self.alignmentPose = [0, 0, 0, 0]
if self.alignmentCounter.value > 0:
self.alignmentPose = list(
map(lambda x, y: x + y, self.alignmentPose, aligned))
self.alignmentCounter.set(self.alignmentCounter.value - 1)
if self.alignmentCounter.value == 0:
self.alignmentPose = quaternion.normalize(self.alignmentPose)
adown = quaternion.rotvecquat([0, 0, 1],
quaternion.conjugate(
self.alignmentPose))
alignment = []
alignment = quaternion.vec2vec2quat([0, 0, 1], adown)
alignment = quaternion.multiply(self.alignmentQ.value,
alignment)
if len(alignment):
self.update_alignment(alignment)
self.last_alignmentCounter = self.alignmentCounter.value
# if alignment or heading offset changed:
if self.heading_off.value != self.heading_off.last or \
self.alignmentQ.value != self.alignmentQ.last:
self.update_alignment(self.alignmentQ.value)
self.heading_off.last = self.heading_off.value
self.alignmentQ.last = self.alignmentQ.value
if self.auto_cal.cal_pipe:
#print('warning, cal pipe always sending despite locks')
cal_data = {}
#how to check this here?? if not 'imu.accel.calibration.locked'
cal_data['accel'] = list(data['accel'])
#how to check this here?? if not 'imu.compass.calibration.locked'
cal_data['compass'] = list(data['compass'])
cal_data['fusionQPose'] = list(data['fusionQPose'])
if cal_data:
#print('send', cal_data)
self.auto_cal.cal_pipe.send(cal_data)
return data
def IMURead(self):
data = False
while self.poller.poll(0): # read all the data from the pipe
data = self.imu_pipe.recv()
if not data:
if time.time() - self.last_imuread > 1 and self.loopfreq.value:
print('IMURead failed!')
self.loopfreq.set(0)
for name in self.SensorValues:
self.SensorValues[name].set(False)
self.uptime.reset()
return False
if vector.norm(data['accel']) == 0:
print('accel values invalid', data['accel'])
return False
t = time.time()
self.timestamp.set(t - self.starttime)
self.last_imuread = t
self.loopfreq.strobe()
# apply alignment calibration
gyro_q = quaternion.rotvecquat(data['gyro'], data['fusionQPose'])
data['pitchrate'], data['rollrate'], data['headingrate'] = map(
math.degrees, gyro_q)
origfusionQPose = data['fusionQPose']
aligned = quaternion.multiply(data['fusionQPose'],
self.alignmentQ.value)
data['fusionQPose'] = quaternion.normalize(
aligned) # floating point precision errors
data['roll'], data['pitch'], data['heading'] = map(
math.degrees, quaternion.toeuler(data['fusionQPose']))
if data['heading'] < 0:
data['heading'] += 360
dt = data['timestamp'] - self.lasttimestamp
self.lasttimestamp = data['timestamp']
if dt > .02 and dt < .5:
data['headingraterate'] = (data['headingrate'] -
self.headingrate) / dt
else:
data['headingraterate'] = 0
self.headingrate = data['headingrate']
data['heel'] = self.heel = data['roll'] * .03 + self.heel * .97
#data['roll'] -= data['heel']
data['gyro'] = list(map(math.degrees, data['gyro']))
data['gyrobias'] = list(map(math.degrees, data['gyrobias']))
# lowpass heading and rate
llp = self.heading_lowpass_constant.value
data['heading_lowpass'] = heading_filter(
llp, data['heading'], self.SensorValues['heading_lowpass'].value)
llp = self.headingrate_lowpass_constant.value
data['headingrate_lowpass'] = llp * data['headingrate'] + (
1 - llp) * self.SensorValues['headingrate_lowpass'].value
llp = self.headingraterate_lowpass_constant.value
data['headingraterate_lowpass'] = llp * data['headingraterate'] + (
1 - llp) * self.SensorValues['headingraterate_lowpass'].value
# set sensors
for name in self.SensorValues:
self.SensorValues[name].set(data[name])
self.uptime.update()
# count down to alignment
if self.alignmentCounter.value != self.last_alignmentCounter:
self.alignmentPose = [0, 0, 0, 0]
if self.alignmentCounter.value > 0:
self.alignmentPose = list(
map(lambda x, y: x + y, self.alignmentPose,
data['fusionQPose']))
self.alignmentCounter.set(self.alignmentCounter.value - 1)
if self.alignmentCounter.value == 0:
self.alignmentPose = quaternion.normalize(self.alignmentPose)
adown = quaternion.rotvecquat([0, 0, 1],
quaternion.conjugate(
self.alignmentPose))
alignment = []
alignment = quaternion.vec2vec2quat([0, 0, 1], adown)
alignment = quaternion.multiply(self.alignmentQ.value,
alignment)
if len(alignment):
self.update_alignment(alignment)
self.last_alignmentCounter = self.alignmentCounter.value
# if alignment or heading offset changed:
if self.heading_off.value != self.heading_off.last or self.alignmentQ.value != self.alignmentQ.last:
self.update_alignment(self.alignmentQ.value)
self.heading_off.last = self.heading_off.value
self.alignmentQ.last = self.alignmentQ.value
cal_data = {}
if not self.accel_calibration.locked.value:
cal_data['accel'] = list(data['accel'])
if not self.compass_calibration.locked.value:
cal_data['compass'] = list(data['compass'])
cal_data['down'] = quaternion.rotvecquat(
[0, 0, 1], quaternion.conjugate(origfusionQPose))
if cal_data:
self.auto_cal.cal_pipe.send(cal_data)
self.accel_calibration.age.update()
self.compass_calibration.age.update()
return data
def process_model(states, dt):
q = quaternion.multiply(states[:, :4],
quaternion.from_rv(states[:, 4:], dt))
w = states[:, 4:]
return np.column_stack((q, w))
