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))