class ComplimentaryFilter: def __init__(self, k=0.5): self.q = Quaternion(1, 0, 0, 0) self.t = 0 self.k = k def update(self, msr): t = msr['t'] dt = t - self.t self.t = t w = Quaternion(0, msr['gyro.x'], msr['gyro.y'], msr['gyro.z']) q_fqa = fqa(msr) dq_static = q_fqa.add((self.q.scalar_multiply(-1))).scalar_multiply(1/dt) dq_dynamic = w.multiply(self.q).scalar_multiply(0.5) dq = dq_static.scalar_multiply(self.k).add(dq_dynamic.scalar_multiply(1 - self.k)) self.q = self.q.add(dq.scalar_multiply(dt)) self.q = Quaternion.normalize(self.q)