diff --git a/msg/templates/urtps/microRTPS_timesync.cpp.em b/msg/templates/urtps/microRTPS_timesync.cpp.em index a013b1ced3..f65c589db6 100644 --- a/msg/templates/urtps/microRTPS_timesync.cpp.em +++ b/msg/templates/urtps/microRTPS_timesync.cpp.em @@ -157,9 +157,15 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t return false; } - double schedule = std::min((double)_num_samples / WINDOW_SIZE, 1.); - double alpha = ALPHA_INITIAL * (1. - schedule) + ALPHA_FINAL * schedule; - double beta = BETA_INTIIAL * (1. - schedule) + BETA_FINAL * schedule; + double alpha = ALPHA_FINAL; + double beta = BETA_FINAL; + + if (_num_samples < WINDOW_SIZE) { + double schedule = (double)_num_samples / WINDOW_SIZE; + double s = 1. - exp(.5 * (1. - 1. / (1. - schedule))); + alpha = (1. - s) * ALPHA_INITIAL + s * ALPHA_FINAL; + beta = (1. - s) * BETA_INITIAL + s * BETA_FINAL; + } int64_t offset_prev = _offset_ns.load(); updateOffset(static_cast((_skew_ns_per_sync + _offset_ns.load()) * (1. - alpha) + measurement_offset * alpha)); diff --git a/msg/templates/urtps/microRTPS_timesync.h.em b/msg/templates/urtps/microRTPS_timesync.h.em index e97041eb71..e4764395a6 100644 --- a/msg/templates/urtps/microRTPS_timesync.h.em +++ b/msg/templates/urtps/microRTPS_timesync.h.em @@ -78,7 +78,7 @@ except AttributeError: static constexpr double ALPHA_INITIAL = 0.05; static constexpr double ALPHA_FINAL = 0.003; -static constexpr double BETA_INTIIAL = 0.05; +static constexpr double BETA_INITIAL = 0.05; static constexpr double BETA_FINAL = 0.003; static constexpr int WINDOW_SIZE = 500; static constexpr int64_t UNKNOWN = 0;