def Update(self, z, t):
if not isinstance(z, float):
z = None
if (self.zf is not None) and (self.t is not None):
if (z is not None) and (t is not None):
zUnwrapped = CircleFunctions.UnwrapHalfCircle(z, self.zf)
dt = t - self.t
tt = (self.RC + dt)
if (tt > 0.0):
alpha = dt / (self.RC + dt)
else:
alpha = 0.0
self.zf = (1.0 - alpha) * self.zf + alpha * zUnwrapped
else: # Initialized, but no measurement.
pass
else: # Not initialized, but have a measurement.
self.zf = z
self.t = t
return self.zf
