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