RemoteUptimeClock.java

Go to the documentation of this file.

/*
 * 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.apache.commons.logging.LogFactory;
import org.ros.exception.RosRuntimeException;

import java.util.Collections;
import java.util.List;
import java.util.Queue;
import java.util.concurrent.Callable;

public class RemoteUptimeClock {

  private static final boolean DEBUG = false;
  private static final Log log = LogFactory.getLog(RemoteUptimeClock.class);

  private final LocalUptimeProvider localUptimeProvider;
  private final Callable<Double> callable;
  private final LatencyOutlierFilter latencyOutlierFilter;

  private final double driftSensitivity;

  private final double errorReductionCoefficientSensitivity;

  private double localUptime;

  private double measuredRemoteUptime;

  private double predictedRemoteUptime;

  private double drift;

  private double errorReductionCoefficient;

  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;
    }
  }

  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();
    }

    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();
  }

  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;
  }

  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));
  }

  private double calculateDrift(double localUptimeDelta, double remoteUptimeDelta) {
    Preconditions.checkState(remoteUptimeDelta > 1e-9);
    return localUptimeDelta / remoteUptimeDelta;
  }

  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 (DEBUG) {
      log.info(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;
  }

  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);
  }

  public double toLocalUptime(double remoteUptime) {
    double localOffset =
        (drift + errorReductionCoefficient) * (remoteUptime - predictedRemoteUptime);
    return localUptime + localOffset;
  }

  @VisibleForTesting
  double getDrift() {
    return drift;
  }

  @VisibleForTesting
  double getErrorReductionCoefficient() {
    return errorReductionCoefficient;
  }
}