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