Java tutorial
/* * Copyright (C) 2011 Google Inc. * * Licensed under the Apache License, Version 2.0 (the "License"); you may not * use this file except in compliance with the License. You may obtain a copy of * the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the * License for the specific language governing permissions and limitations under * the License. */ package org.ros.time; import com.google.common.annotations.VisibleForTesting; import com.google.common.base.Preconditions; import com.google.common.collect.Lists; import org.apache.commons.logging.Log; import org.ros.exception.RosRuntimeException; import org.ros.log.RosLogFactory; import java.util.Collections; import java.util.List; import java.util.Queue; import java.util.concurrent.Callable; /** * @author damonkohler@google.com (Damon Kohler) */ public class RemoteUptimeClock { private static final Log log = RosLogFactory.getLog(RemoteUptimeClock.class); private final LocalUptimeProvider localUptimeProvider; private final Callable<Double> callable; private final LatencyOutlierFilter latencyOutlierFilter; /** * Sensitivity values are used to sampleSize the effect of jitter. The value * should be in the range [0, 1] where 0 indicates that the current estimate * will never change (i.e. new measurements have no effect on estimates) and 1 * indicates that previous estimates have no effect on changes to the current * estimate. */ private final double driftSensitivity; /** * @see #driftSensitivity */ private final double errorReductionCoefficientSensitivity; private double localUptime; /** * Remote uptime is tracked as a pair of values: our previous measurement and * our prediction based on estimated drift. */ private double measuredRemoteUptime; /** * @see #measuredRemoteUptime */ private double predictedRemoteUptime; /** * Drift is measured in local uptime ticks per remote uptime tick. * * @see #calculateDrift(double, double) */ private double drift; /** * With {@link #drift} alone, it is possible to accumulate a constant error * that will never be corrected for. The {@link #errorReductionCoefficient} is * an additional term for removing this error. */ private double errorReductionCoefficient; /** * Represents a tuple of measurement values that represent a single point in * time. */ private final class UptimeCalculationResult { final double newLocalUptime; final double newRemoteUptime; final double latency; public UptimeCalculationResult(double newLocalUptime, double newRemoteUptime, double latency) { this.newLocalUptime = newLocalUptime; this.newRemoteUptime = newRemoteUptime; this.latency = latency; } } /** * Uses a sliding window and percentile range to detect latency outliers. * * <p> * When receiving remote uptime measurements, the latency of the measurement * is used to estimate the local uptime at the point when the remote uptime * was measured. This calculation assumes that any measurement latency is * symmetrical. The larger the latency, the larger the potential error in our * estimate of local uptime at the measured remote uptime. * * <p> * To reduce the effect of measurements with higher uncertainty, we filter out * measurements with latencies that exceed the specified percentile within our * sliding window. */ private final class LatencyOutlierFilter { private final int sampleSize; private final double threshold; private final Queue<Double> latencies; public LatencyOutlierFilter(int sampleSize, double threshold) { Preconditions.checkArgument(sampleSize > 0); Preconditions.checkArgument(threshold > 1); this.threshold = threshold; this.sampleSize = sampleSize; latencies = Lists.newLinkedList(); } /** * @param latency * @return {@code true} if the provided latency is outside the configured * percentile, {@code false} otherwise */ public boolean add(double latency) { latencies.add(latency); if (latencies.size() > sampleSize) { latencies.remove(); } else { // Until the sliding window is full, we cannot reliably detect // outliers. return false; } double medianLatency = getMedian(); if (latency < medianLatency * threshold) { return false; } return true; } public double getMedian() { List<Double> ordered = Lists.newArrayList(latencies); Collections.sort(ordered); return ordered.get(latencies.size() / 2); } } @VisibleForTesting interface LocalUptimeProvider { double getSeconds(); } /** * The provided {@link Callable} should return the current * measuredRemoteUptime of the remote clock with minimal overhead since the * run time of this call will be used to further improve the estimation of * measuredRemoteUptime. * * @param timeProvider * the local time provider * @param callable * returns the current remote uptime in arbitrary units * @param driftSensitivity * the sensitivity to drift adjustments, must be in the range [0, 1] * @param errorReductionCoefficientSensitivity * the sensitivity to error reduction coefficient adjustments, must * be in the range [0, 1] * @return a new {@link RemoteUptimeClock} */ public static RemoteUptimeClock newDefault(final TimeProvider timeProvider, Callable<Double> callable, double driftSensitivity, double errorReductionCoefficientSensitivity, int latencyOutlierFilterSampleSize, double latencyOutlierFilterThreshold) { return new RemoteUptimeClock(new LocalUptimeProvider() { @Override public double getSeconds() { return timeProvider.getCurrentTime().toSeconds(); } }, callable, driftSensitivity, errorReductionCoefficientSensitivity, latencyOutlierFilterSampleSize, latencyOutlierFilterThreshold); } @VisibleForTesting RemoteUptimeClock(LocalUptimeProvider localUptimeProvider, Callable<Double> callable, double driftSensitivity, double errorReductionCoefficientSensitivity, int latencyOutlierFilterSampleSize, double latencyOutlierFilterThreshold) { Preconditions.checkArgument(driftSensitivity >= 0 && driftSensitivity <= 1); Preconditions.checkArgument( errorReductionCoefficientSensitivity >= 0 && errorReductionCoefficientSensitivity <= 1); this.localUptimeProvider = localUptimeProvider; this.callable = callable; this.driftSensitivity = driftSensitivity; this.errorReductionCoefficientSensitivity = errorReductionCoefficientSensitivity; latencyOutlierFilter = new LatencyOutlierFilter(latencyOutlierFilterSampleSize, latencyOutlierFilterThreshold); errorReductionCoefficient = 0; } /** * Good calibration settings will depend on the remote uptime provider. In * general, choosing a sample size around 10 and a delay that is large enough * to include more than 100 uptime ticks will give reasonable results. * * @param sampleSize * the number of samples to use for calibration * @param samplingDelayMillis * the delay in milliseconds between collecting each sample */ public void calibrate(int sampleSize, double samplingDelayMillis) { log.info("Starting calibration..."); double remoteUptimeSum = 0; double localUptimeSum = 0; double driftSum = 0; for (int i = 0; i < sampleSize; i++) { UptimeCalculationResult result = calculateNewUptime(callable); latencyOutlierFilter.add(result.latency); if (i > 0) { double localUptimeDelta = result.newLocalUptime - localUptime; double remoteUptimeDelta = result.newRemoteUptime - measuredRemoteUptime; driftSum += calculateDrift(localUptimeDelta, remoteUptimeDelta); } measuredRemoteUptime = result.newRemoteUptime; localUptime = result.newLocalUptime; remoteUptimeSum += measuredRemoteUptime; localUptimeSum += localUptime; try { Thread.sleep((long) samplingDelayMillis); } catch (InterruptedException e) { throw new RosRuntimeException(e); } } // We have n samples, but n - 1 intervals. errorReductionCoefficient is // the // average interval magnitude. drift = driftSum / (sampleSize - 1); // If localUptime == -offset then measuredRemoteUptime == 0 (e.g. if // localUptime is 10s and measuredRemoteUptime is 5s, then offset should // be // -5s since the localUptime started 5s earlier than // measuredRemoteUptime). double offset = (drift * remoteUptimeSum - localUptimeSum) / sampleSize; predictedRemoteUptime = (localUptime + offset) / drift; log.info(String.format("Calibration complete. Drift: %.4g, Offset: %.4f s", drift, offset)); } /** * @see #drift * * @param localUptimeDelta * the delta between the two local uptimes that correspond to the two * remote uptimes used to determine {@code remoteUptimeDelta} * @param remoteUptimeDelta * the delta between the two remote uptimes that correspond to the * two local uptimes used to determine {@code localUptimeDelta} * @return the calculated drift */ private double calculateDrift(double localUptimeDelta, double remoteUptimeDelta) { Preconditions.checkState(remoteUptimeDelta > 1e-9); return localUptimeDelta / remoteUptimeDelta; } /** * Update this {@link RemoteUptimeClock} with the latest uptime from the * remote clock. * * <p> * This will update internal estimates of drift and error. Ideally, it should * be called periodically with a consistent time interval between updates * (e.g. 10 seconds). */ public void update() { UptimeCalculationResult result = calculateNewUptime(callable); double newLocalUptime = result.newLocalUptime; double newRemoteUptime = result.newRemoteUptime; double latency = result.latency; if (latencyOutlierFilter.add(latency)) { log.warn(String.format("Measurement latency marked as outlier. Latency: %.4f s, Median: %.4f s", latency, latencyOutlierFilter.getMedian())); return; } double localUptimeDelta = newLocalUptime - localUptime; double remoteUptimeDelta = newRemoteUptime - measuredRemoteUptime; Preconditions.checkState(localUptimeDelta > 1e-9); Preconditions.checkState(remoteUptimeDelta > 1e-9); if (log.isDebugEnabled()) { log.debug(String.format("localUptimeDelta: %.4g, remoteUptimeDelta: %.4g", localUptimeDelta, remoteUptimeDelta)); } double newDrift = driftSensitivity * (localUptimeDelta / remoteUptimeDelta) + (1 - driftSensitivity) * drift; // Non-jumping behavior from (localUptime, predictedRemoteUptime) to // (newLocalUptime, newAdjustedRemoteUptime). Note that it does not // depend // directly on measuredRemoteUptime or newRemoteUptime. double newPredictedRemoteUptime = predictedRemoteUptime + (localUptimeDelta / (drift + errorReductionCoefficient)); double nextPredictedRemoteUptime = newRemoteUptime + remoteUptimeDelta; double newCombinedDriftAndError = localUptimeDelta / (nextPredictedRemoteUptime - newPredictedRemoteUptime); double newErrorReductionCoefficient = errorReductionCoefficientSensitivity * (newCombinedDriftAndError - newDrift); double deltaRatio = remoteUptimeDelta / localUptimeDelta; double error = newLocalUptime - toLocalUptime(newRemoteUptime); log.info(String.format( "Latency: %.4f s, Delta ratio: %.4f, Drift: %.4g, " + "Error reduction coefficient: %.4g, Error: %.4f s", latency, deltaRatio, newDrift, newErrorReductionCoefficient, error)); measuredRemoteUptime = newRemoteUptime; predictedRemoteUptime = newPredictedRemoteUptime; localUptime = newLocalUptime; drift = newDrift; errorReductionCoefficient = newErrorReductionCoefficient; } /** * Creates a new {@link UptimeCalculationResult} where the local uptime has * been adjusted to compensate for latency while retrieving the remote uptime. * * @param callable * returns the remote uptime as quickly as possible * @return a new {@link UptimeCalculationResult} */ private UptimeCalculationResult calculateNewUptime(Callable<Double> callable) { double newLocalUptime = localUptimeProvider.getSeconds(); double newRemoteUptime; try { newRemoteUptime = callable.call(); } catch (Exception e) { log.error(e); throw new RosRuntimeException(e); } double latency = localUptimeProvider.getSeconds() - newLocalUptime; double latencyOffset = latency / 2; newLocalUptime += latencyOffset; return new UptimeCalculationResult(newLocalUptime, newRemoteUptime, latency); } /** * Returns the estimated local uptime in seconds for the given remote uptime. * * @param remoteUptime * the remote uptime to convert to local uptime * @return the estimated local uptime in seconds at the provided remote uptime */ public double toLocalUptime(double remoteUptime) { double localOffset = (drift + errorReductionCoefficient) * (remoteUptime - predictedRemoteUptime); return localUptime + localOffset; } @VisibleForTesting double getDrift() { return drift; } @VisibleForTesting double getErrorReductionCoefficient() { return errorReductionCoefficient; } }