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