Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Olson latency estimator class #80

Open
pavel-kirienko opened this issue Oct 5, 2019 · 0 comments

Comments

@pavel-kirienko
Copy link
Member

commented Oct 5, 2019

Add a class containing a real-time (non-bidirectional) implementation of the Olson algorithm into pyuavcan.util._olson_latency_estimator.py.

Paper: https://files.zubax.com/products/com.zubax.babel/olson_sensor_synchronization.pdf
One time synchronization, search for "Olson": https://forum.uavcan.org/t/alternative-transport-protocols/324
The old implementation can be found here (the source time resolver class is irrelevant and actually harmful because it breaks encapsulation):

class TimestampEstimator:
"""
Based on "A Passive Solution to the Sensor Synchronization Problem" [Edwin Olson 2010]
https://april.eecs.umich.edu/pdfs/olson2010.pdf
"""
DEFAULT_MAX_DRIFT_PPM = 200
DEFAULT_MAX_PHASE_ERROR_TO_RESYNC = 1.
def __init__(self,
max_rate_error=None,
source_clock_overflow_period=None,
fixed_delay=None,
max_phase_error_to_resync=None):
"""
Args:
max_rate_error: The max drift parameter must be not lower than maximum relative clock
drift in PPM. If the max relative drift is guaranteed to be lower,
reducing this value will improve estimation. The default covers vast
majority of low-cost (and up) crystal oscillators.
source_clock_overflow_period: How often the source clocks wraps over, in seconds.
For example, for SLCAN this value is 60 seconds.
If not provided, the source clock is considered to never wrap over.
fixed_delay: This value will be unconditionally added to the delay estimations.
Represented in seconds. Default is zero.
For USB-interfaced sources it should be safe to use as much as 100 usec.
max_phase_error_to_resync: When this value is exceeded, the estimator will start over.
Defaults to a large value.
"""
self.max_rate_error = float(max_rate_error or (self.DEFAULT_MAX_DRIFT_PPM / 1e6))
self.fixed_delay = fixed_delay or 0
self.max_phase_error_to_resync = max_phase_error_to_resync or self.DEFAULT_MAX_PHASE_ERROR_TO_RESYNC
if self.max_rate_error < 0:
raise ValueError('max_rate_error must be non-negative')
if self.fixed_delay < 0:
raise ValueError('fixed_delay must be non-negative')
if self.max_phase_error_to_resync <= 0:
raise ValueError('max_phase_error_to_resync must be positive')
# This is used to recover absolute source time
self._source_time_resolver = SourceTimeResolver(source_clock_overflow_period=source_clock_overflow_period)
# Refer to the paper for explanations
self._p = None
self._q = None
# Statistics
self._estimated_delay = 0.0
self._resync_count = 0
def update(self, source_clock_sample, target_clock_sample):
"""
Args:
source_clock_sample: E.g. value received from the source system, in seconds
target_clock_sample: E.g. target time sampled when the data arrived to the local system, in seconds
Returns: Event timestamp converted to the target time domain.
"""
pi = float(self._source_time_resolver.update(source_clock_sample, target_clock_sample))
qi = target_clock_sample
# Initialization
if self._p is None:
self._p = pi
self._q = qi
# Sync error - refer to the reference implementation of the algorithm
self._estimated_delay = abs((pi - self._p) - (qi - self._q))
# Resynchronization (discarding known state)
if self._estimated_delay > self.max_phase_error_to_resync:
self._source_time_resolver.reset()
self._resync_count += 1
self._p = pi = float(self._source_time_resolver.update(source_clock_sample, target_clock_sample))
self._q = qi
# Offset options
assert pi >= self._p
offset = self._p - self._q - self.max_rate_error * (pi - self._p) - self.fixed_delay
new_offset = pi - qi - self.fixed_delay
# Updating p/q if the new offset is lower by magnitude
if new_offset >= offset:
offset = new_offset
self._p = pi
self._q = qi
ti = pi - offset
return ti
@property
def estimated_delay(self):
"""Estimated delay, updated in the last call to update()"""
return self._estimated_delay
@property
def resync_count(self):
return self._resync_count

The new implementation need not be API compatible with the old one, obviously; it's just an example.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
1 participant
You can’t perform that action at this time.