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)
