def predict(self, accel_ned_magnetic, t): # log new prediction and apply it estimating new state # subtract gravity residual_accel_magnetic = vector.sub(accel_ned_magnetic, [0, 0, 1]) # rotate by declination decl_q = quaternion.angvec2quat(self.declination.value, [0, 0, 1]) accel_true = quaternion.rotvecquat(residual_accel_magnetic, decl_q) # apply predicted compass error error_q = quaternion.angvec2quat(self.compass_offset.value, [0, 0, 1]) accel_ned = quaternion.rotvecquat(accel_true, error_q) U = 9.81 * np.array(accel_ned) if not self.use3d: U = U[:2] t = time.monotonic() dt = t - self.predict_t self.predict_t = t if dt < 0 or dt > .5: self.reset() if not self.X: # do not have a trusted measurement yet, so cannot perform predictions return self.apply_prediction(dt, U) self.history.append({ 't': t, 'dt': dt, 'U': U, 'X': self.X, 'P': self.P }) # filtered position ll = xy_to_ll(self.X[0], self.X[1], *self.lastll) self.lat.set(ll[0]) self.lon.set(ll[1]) # filtered speed and track c = 3 if self.use3d else 2 vx = self.X[c] vy = self.X[c + 1] self.speed.set(math.hypot(vx, vy)) self.track.set(resolv(math.degrees(math.atan2(vx, vy)), 180)) # filtered altitude and climb if self.use3d: self.alt.set(self.X[2]) self.climb.set(self.X[5])
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 predict(self, ap): if not self.source.value == 'none': return accel = ap.boatimu.SensorValues['accel'].value fusionQPose = ap.boatimu.SensorValues['fusionQPose'].value if accel and fusionQPose: self.filtered.predict(quaternion.rotvecquat(accel, fusionQPose), time.monotonic())
def CalibrationProcess(cal_pipe, client): accel_points = SigmaPoints(.05**2, 12, 10) compass_points = SigmaPoints(1.1**2, 28, 3) norm = [0, 0, 1] accel_calibration = RegisterCalibration(client, 'imu.accel', [[0, 0, 0, 1], 1]) compass_calibration = RegisterCalibration(client, 'imu.compass', [[0, 0, 0, 30, 0], [1, 1], 0]) client.watch('imu.alignmentQ') if not cal_pipe: # get these through client rather than direct pipe client.watch('imu.accel') client.watch('imu.compass') client.watch('imu.fusionQPose') def debug(name): def debug_by_name(*args): s = '' for a in args: s += str(a) + ' ' client.set('imu.'+name+'.calibration.log', s) return debug_by_name last_compass_coverage = 0 while True: t = time.monotonic() addedpoint = False down = False while time.monotonic() - t < calibration_fit_period: # receive pypilot messages msg = client.receive(1) for name in msg: value = msg[name] if name == 'imu.alignmentQ' and value: norm = quaternion.rotvecquat([0, 0, 1], value) compass_points.Reset() elif name == 'imu.accel': if value: accel_points.AddPoint(value) addedpoint = True elif name == 'imu.compass' and down: if value and down: compass_points.AddPoint(value, down) addedpoint = True elif name == 'imu.fusionQPose': if value: down = quaternion.rotvecquat([0, 0, 1], quaternion.conjugate(value)) # receive calibration data if cal_pipe: p = cal_pipe.recv() while p: if 'accel' in p: accel_points.AddPoint(p['accel']) addedpoint = True if 'compass' in p: down = quaternion.rotvecquat([0, 0, 1], quaternion.conjugate(p['fusionQPose'])) compass_points.AddPoint(p['compass'], down) addedpoint = True p = cal_pipe.recv() cals = [(accel_calibration, accel_points), (compass_calibration, compass_points)] for calibration, points in cals: calibration.age.update() if points.Updated(): calibration.sigmapoints.set(points.Points()) if not addedpoint: # don't bother to run fit if no new data continue accel_points.RemoveOlder(10*60) # 10 minutes fit = FitAccel(debug('accel'), accel_points) if fit: # reset compass sigmapoints on accel cal dist = vector.dist(fit[0][:3], accel_calibration.value[0][:3]) if dist > .01: # only update when bias changes more than this if dist > .08: # reset compass cal from large change in accel bias compass_points.Reset() debug('accel')('reset compass from large accel bias') accel_calibration.set(fit) accel_calibration.points.set(accel_points.Points()) else: debug('accel')('calibration distance too small ', dist) compass_points.RemoveOlder(20*60) # 20 minutes fit = FitCompass(debug('compass'), compass_points, compass_calibration.value[0], norm) if fit: # ignore decreasing compass coverage new_coverage = fit[1][2] if new_coverage < last_compass_coverage: debug('compass')('ignoring decreasing coverage') else: compass_calibration.set(fit) compass_calibration.points.set(compass_points.Points()) else: last_compass_coverage = 0 # reset
def ang(p): c = quaternion.rotvecquat(vector.sub(p[:3], bias), q) d = quaternion.rotvecquat(p[3:6], q) v = quaternion.rotvecquat(c, quaternion.vec2vec2quat(d, [0, 0, 1])) v = vector.normalize(v) return math.degrees(math.atan2(v[1], v[0]))
def CalibrationProcess(cal_pipe): import os if os.system('sudo chrt -pi 0 %d 2> /dev/null > /dev/null' % os.getpid()): print( 'warning, failed to make calibration process idle, trying renice') if os.system("renice 20 %d" % os.getpid()): print('warning, failed to renice calibration process') accel_cal = SigmaPoints(.05**2, 12, 10) compass_cal = SigmaPoints(1.1**2, 24, 3) accel_calibration = [0, 0, 0, 1] norm = [0, 0, 1] def on_con(client): client.watch('imu.alignmentQ') client.watch('imu.compass.calibration') while True: time.sleep(2) try: client = pypilotClient(on_con, 'localhost', autoreconnect=True) break except Exception as e: print('nmea process failed to connect pypilot', e) def debug(name): def debug_by_name(*args): s = '' for a in args: s += str(a) + ' ' if client: client.set('imu.' + name + '.calibration.log', s) return debug_by_name while True: t = time.time() addedpoint = False while time.time() - t < calibration_fit_period: # receive pypilot messages msg = client.receive() for name in msg: value = msg[name]['value'] if name == 'imu.alignmentQ' and value: norm = quaternion.rotvecquat([0, 0, 1], value) compass_cal.Reset() elif name == 'imu.compass.calibration': compass_calibration = value[0] # receive calibration data p = cal_pipe.recv(1) if p: if 'accel' in p: accel_cal.AddPoint(p['accel']) addedpoint = True if 'compass' in p: compass_cal.AddPoint(p['compass'], p['down']) addedpoint = True # send updated sigmapoints as well cals = {'accel': accel_cal, 'compass': compass_cal} for name in cals: cal = cals[name] if cal.Updated(): points = cal.Points() client.set('imu.' + name + '.calibration.sigmapoints', points) if not addedpoint: # don't bother to run fit if no new data continue accel_cal.RemoveOlder(10 * 60) # 10 minutes fit = FitAccel(debug('accel'), accel_cal) if fit: # reset compass sigmapoints on accel cal dist = vector.dist(fit[0][:3], accel_calibration[:3]) if dist > .01: # only update when bias changes more than this if dist > .08: # reset compass cal from large change in accel bias compass_cal.Reset() accel_calibration = fit[0] client.set('imu.accel.calibration', fit) compass_cal.RemoveOlder(20 * 60) # 20 minutes fit = FitCompass(debug('compass'), compass_cal, compass_calibration, norm) if fit: client.set('imu.compass.calibration', fit) compass_calibration = fit[0]
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 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