diff --git a/aos/network/multinode_timestamp_filter_test.cc b/aos/network/multinode_timestamp_filter_test.cc
index 1c1ca54..07d3712 100644
--- a/aos/network/multinode_timestamp_filter_test.cc
+++ b/aos/network/multinode_timestamp_filter_test.cc
@@ -180,7 +180,7 @@
   problem.add_filter(0, &a, 1);
   problem.add_filter(1, &b, 0);
 
-  // Solve the problem with infinate precision as a verification and compare the
+  // Solve the problem with infinite precision as a verification and compare the
   // result.
   {
     const std::vector<double> result = problem.SolveDouble();
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index c623cbe..5e4d115 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -499,6 +499,7 @@
                 (std::get<0>(sample) + std::get<1>(sample)).time_since_epoch())
                 .count());
   }
+  fflush(samples_fp_);
   saved_samples_.clear();
 }
 
@@ -520,6 +521,7 @@
 NoncausalTimestampFilter::FindTimestamps(monotonic_clock::time_point ta) const {
   CHECK_GT(timestamps_size(), 1u);
   // Linear search until this is proven to be a measurable slowdown.
+  // If ta is outside our timestamp range, return the closest pair
   size_t index = 0;
   while (index < timestamps_size() - 2u) {
     if (std::get<0>(timestamp(index + 1)) > ta) {
@@ -530,6 +532,36 @@
   return std::make_pair(timestamp(index), timestamp(index + 1));
 }
 
+chrono::nanoseconds NoncausalTimestampFilter::ExtrapolateOffset(
+    std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
+    monotonic_clock::time_point ta) {
+  const chrono::nanoseconds dt = ta - std::get<0>(p0);
+  if (dt <= std::chrono::nanoseconds(0)) {
+    // Extrapolate backwards, using the (positive) MaxVelocity slope
+    // We've been asked to extrapolate the offset to a time before our first
+    // sample point.  To be conservative, we'll return an extrapolated
+    // offset that is less than (less tight an estimate of the network delay)
+    // than our sample offset, bound by the max slew velocity we allow
+    //       p0
+    //      /
+    //     /
+    //   ta
+    // Since dt < 0, we shift by dt * slope to get that value
+    return std::get<1>(p0) +
+           chrono::duration_cast<chrono::nanoseconds>(dt * kMaxVelocity());
+  } else {
+    // Extrapolate forwards, using the (negative) MaxVelocity slope
+    // Same concept, except going foward past our last (most recent) sample:
+    //       pN
+    //         |
+    //          |
+    //           ta
+    // Since dt > 0, we shift by - dt * slope to get that value
+    return std::get<1>(p0) -
+           chrono::duration_cast<chrono::nanoseconds>(dt * kMaxVelocity());
+  }
+}
+
 chrono::nanoseconds NoncausalTimestampFilter::InterpolateOffset(
     std::tuple<monotonic_clock::time_point, chrono::nanoseconds> p0,
     std::tuple<monotonic_clock::time_point, chrono::nanoseconds> p1,
@@ -596,13 +628,78 @@
          (std::get<1>(p1) - std::get<1>(p0)).count();
 }
 
+chrono::nanoseconds NoncausalTimestampFilter::ExtrapolateOffset(
+    std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
+    monotonic_clock::time_point /*ta_base*/, double /*ta*/) {
+  // For this version, use the base offset from p0 as the base for the offset
+  return std::get<1>(p0);
+}
+
+double NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
+    std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
+    monotonic_clock::time_point ta_base, double ta) {
+  // Compute the remainder portion of this offset
+  // oa = p0.o +/- ((ta + ta_base) - p0.t)) * kMaxVelocity()
+  //               ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+  // But compute (ta + ta_base - p0.t) as (ta + (ta_base - p0.t))
+  // to handle numerical precision
+  const chrono::nanoseconds time_in = ta_base - std::get<0>(p0);
+  const double dt = static_cast<double>(ta + time_in.count());
+  if (dt < 0.0) {
+    // Extrapolate backwards with max (positive) slope (which means
+    // the returned offset should be negative)
+    return dt * kMaxVelocity();
+  } else {
+    // Extrapolate forwards with max (negative) slope
+    return -dt * kMaxVelocity();
+  }
+}
+
+bool NoncausalTimestampFilter::IsOutsideSamples(
+    monotonic_clock::time_point ta_base, double ta) const {
+  DCHECK_GE(ta, 0.0);
+  DCHECK_LT(ta, 1.0);
+  if (timestamps_size() == 1u || ta_base < std::get<0>(timestamp(0)) ||
+      ta_base >= std::get<0>(timestamp(timestamps_size() - 1u))) {
+    return true;
+  }
+
+  return false;
+}
+
+bool NoncausalTimestampFilter::IsAfterSamples(
+    monotonic_clock::time_point ta_base, double ta) const {
+  DCHECK_GE(ta, 0.0);
+  DCHECK_LT(ta, 1.0);
+  if (ta_base >= std::get<0>(timestamp(timestamps_size() - 1u))) {
+    return true;
+  }
+
+  return false;
+}
+
+std::tuple<monotonic_clock::time_point, chrono::nanoseconds>
+NoncausalTimestampFilter::GetReferenceTimestamp(
+    monotonic_clock::time_point ta_base, double ta) const {
+  DCHECK_GE(ta, 0.0);
+  DCHECK_LT(ta, 1.0);
+  std::tuple<monotonic_clock::time_point, chrono::nanoseconds>
+      reference_timestamp = timestamp(0);
+  if (ta_base >= std::get<0>(timestamp(timestamps_size() - 1u))) {
+    reference_timestamp = timestamp(timestamps_size() - 1u);
+  }
+
+  return reference_timestamp;
+}
+
 chrono::nanoseconds NoncausalTimestampFilter::Offset(
     monotonic_clock::time_point ta) const {
   CHECK_GT(timestamps_size(), 0u);
-  if (timestamps_size() == 1u || ta < std::get<0>(timestamp(0))) {
-    // Special case size = 1 since the interpolation functions don't need to
-    // handle it and the answer is trivial.
-    return NoncausalTimestampFilter::InterpolateOffset(timestamp(0), ta);
+  if (IsOutsideSamples(ta, 0.)) {
+    // Special case when size = 1 or if we're asked to extrapolate to
+    // times before or after we have data.
+    auto reference_timestamp = GetReferenceTimestamp(ta, 0.);
+    return NoncausalTimestampFilter::ExtrapolateOffset(reference_timestamp, ta);
   }
 
   std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
@@ -615,12 +712,14 @@
 std::pair<chrono::nanoseconds, double> NoncausalTimestampFilter::Offset(
     monotonic_clock::time_point ta_base, double ta) const {
   CHECK_GT(timestamps_size(), 0u);
-  if (timestamps_size() == 1u || ta_base < std::get<0>(timestamp(0))) {
-    // Special case size = 1 since the interpolation functions don't need to
-    // handle it and the answer is trivial.
-    return std::make_pair(
-        NoncausalTimestampFilter::InterpolateOffset(timestamp(0), ta_base, ta),
-        0.0);
+  if (IsOutsideSamples(ta_base, ta)) {
+    // Special case size = 1 or ta_base before first timestamp or
+    // after last timesteamp, so we need to extrapolate out
+    auto reference_timestamp = GetReferenceTimestamp(ta_base, ta);
+    return std::make_pair(NoncausalTimestampFilter::ExtrapolateOffset(
+                              reference_timestamp, ta_base, ta),
+                          NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
+                              reference_timestamp, ta_base, ta));
   }
 
   std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
@@ -680,7 +779,7 @@
   // We don't actually want to compute tb - (1 + ma) ta for numerical precision
   // reasons.  The important digits are small compared to the offset.  Given our
   // original cost above, this is equivalent to:
-  //   2 * (tb - ta - Offset(ta)) * (-(1 + ma))
+  //   - 2 * (tb - ta - Offset(ta)) * (1 + ma)
   //
   // We can compute this a lot more accurately.
 
@@ -690,8 +789,20 @@
   NormalizeTimestamps(&ta_base, &ta);
   NormalizeTimestamps(&tb_base, &tb);
 
-  if (timestamps_size() == 1u || ta_base < std::get<0>(timestamp(0))) {
-    return -2.0 * OffsetError(ta_base, ta, tb_base, tb);
+  if (IsOutsideSamples(ta_base, ta)) {
+    double slope = kMaxVelocity();
+    if (IsAfterSamples(ta_base, ta)) {
+      // If we're past the last sample point, use a negative slope
+      slope = -kMaxVelocity();
+    }
+    // This implements the high precision version of the above equation:
+    //   - 2 * (tb - ta - Offset(ta)) * (1 + ma)
+    //      = -2 * OffsetError(ta,tb) * (1 + ma)
+    // Where for extrapolated data, we have to extrapolate for Offset(ta)
+    // and use +/- kMaxVelocity() for ma
+    // (slope is positive if timepoint is before our first sample, and
+    //   negative if the point is after our last sample)
+    return -2.0 * OffsetError(ta_base, ta, tb_base, tb) * (1.0 + slope);
   }
 
   std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
@@ -708,6 +819,8 @@
   //
   //   (tb - ta - Offset(ta)) ->
   //      (tb_base - ta_base - OffsetBase + tb - ta - OffsetRemainder)
+  // NOTE: We don't use OffsetError function here, in order to avoid
+  // extra calls to FindTimestamps
   return -2.0 *
          (static_cast<double>((tb_base - ta_base -
                                NoncausalTimestampFilter::InterpolateOffset(
@@ -726,9 +839,21 @@
   NormalizeTimestamps(&ta_base, &ta);
   NormalizeTimestamps(&tb_base, &tb);
 
-  if (timestamps_size() == 1u || ta_base < std::get<0>(timestamp(0))) {
-    return absl::StrFormat("-2. * (t%d - t%d - %d)", node_b, node_a,
-                           std::get<1>(timestamp(0)).count());
+  if (IsOutsideSamples(ta_base, ta)) {
+    auto reference_timestamp = GetReferenceTimestamp(ta_base, ta);
+    double slope = kMaxVelocity();
+    std::string note = "_";
+    if (IsAfterSamples(ta_base, ta)) {
+      slope = -kMaxVelocity();
+      note = "^";
+    }
+    // d cost / dta ==>
+    // - 2 * (tb - ta - (ta - ref) * ma - ref_offset) * (1 + ma)
+    return absl::StrFormat(
+        "-2. * (t%d - t%d - ((t%d - %d) * %f - %d.) * (1 + %f.)%s", node_b,
+        node_a, node_a,
+        std::get<0>(reference_timestamp).time_since_epoch().count(), slope,
+        std::get<1>(reference_timestamp).count(), slope, note);
   }
 
   std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
@@ -786,9 +911,20 @@
   NormalizeTimestamps(&ta_base, &ta);
   NormalizeTimestamps(&tb_base, &tb);
 
-  if (timestamps_size() == 1u || ta_base < std::get<0>(timestamp(0))) {
-    return absl::StrFormat("2. * (t%d - t%d - %d)", node_b, node_a,
-                           std::get<1>(timestamp(0)).count());
+  if (IsOutsideSamples(ta_base, ta)) {
+    auto reference_timestamp = GetReferenceTimestamp(ta_base, ta);
+    double slope = kMaxVelocity();
+    std::string note = "_";
+    if (IsAfterSamples(ta_base, ta)) {
+      slope = -kMaxVelocity();
+      note = "^";
+    }
+    // d cost / dtb ==> 2 * OffsetError(ta, tb) ==>
+    // 2 * (tb - ta - (ta - ref) * ma - ref_offset)
+    return absl::StrFormat(
+        "2. * (t%d - t%d - ((t%d - %d) * %f + %d)%s", node_b, node_a, node_a,
+        std::get<0>(reference_timestamp).time_since_epoch().count(), slope,
+        std::get<1>(reference_timestamp).count(), note);
   }
 
   std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
@@ -800,7 +936,7 @@
   // ie
   //   ((tb - (1 + ma) ta - ba)^2
   //
-  // d cost/dta =>
+  // d cost/dtb => 2 * OffsetError(ta, tb) ==>
   //   2 * ((tb - (1 + ma) ta - ba)
 
   const int64_t rise =
@@ -827,9 +963,20 @@
   NormalizeTimestamps(&ta_base, &ta);
   NormalizeTimestamps(&tb_base, &tb);
 
-  if (timestamps_size() == 1u || ta_base < std::get<0>(timestamp(0))) {
-    return absl::StrFormat("(t%d - t%d - %d) ** 2.", node_b, node_a,
-                           std::get<1>(timestamp(0)).count());
+  if (IsOutsideSamples(ta_base, ta)) {
+    auto reference_timestamp = GetReferenceTimestamp(ta_base, ta);
+    double slope = kMaxVelocity();
+    std::string note = "_";
+    if (IsAfterSamples(ta_base, ta)) {
+      slope = -kMaxVelocity();
+      note = "^";
+    }
+    // Cost = OffsetError(ta, tb) ** 2 = (tb - ta - Offset(ta, tb)) ** 2 ==>
+    // (tb - ta - ((ta - ref) * ma - ref_offset)
+    return absl::StrFormat(
+        "(t%d - t%d - (t%d - %d.) * %f - %d.)%s ** 2.", node_b, node_a, node_a,
+        std::get<0>(reference_timestamp).time_since_epoch().count(), slope,
+        std::get<1>(reference_timestamp).count(), note);
   }
 
   std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
@@ -865,11 +1012,14 @@
     aos::monotonic_clock::time_point ta,
     aos::monotonic_clock::time_point tb) const {
   CHECK_GT(timestamps_size(), 0u);
-  if (timestamps_size() == 1u || ta < std::get<0>(timestamp(0))) {
-    // Special case size = 1 since the interpolation functions don't need to
-    // handle it and the answer is trivial.
+  if (IsOutsideSamples(ta, 0.)) {
+    // Special case size = 1 or ta_base before first timestamp or
+    // after last timestamp, so we need to extrapolate out
+    auto reference_timestamp = GetReferenceTimestamp(ta, 0.);
+
+    // Special case size = 1 or ta before first timestamp, so we extrapolate
     const chrono::nanoseconds offset =
-        NoncausalTimestampFilter::InterpolateOffset(timestamp(0), ta);
+        NoncausalTimestampFilter::ExtrapolateOffset(reference_timestamp, ta);
     if (offset + ta > tb) {
       LOG(ERROR) << node_a_->name()->string_view() << " -> "
                  << node_b_->name()->string_view() << " "
@@ -1271,6 +1421,7 @@
             std::chrono::duration_cast<std::chrono::duration<double>>(
                 std::get<1>(timestamp))
                 .count());
+    fflush(fp_);
   }
 }
 
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index 49a4d9f..5d577a7 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -14,6 +14,9 @@
 namespace aos {
 namespace message_bridge {
 
+// TODO<jim>: Should do something to help with precision, like make it an
+// integer and divide by the value (e.g., / 1000)
+
 // Max velocity to clamp the filter to in seconds/second.
 inline constexpr double kMaxVelocity() { return 0.001; }
 
@@ -239,6 +242,16 @@
       : node_a_(node_a), node_b_(node_b) {}
   ~NoncausalTimestampFilter();
 
+  // Check whether the given timestamp falls within our current samples
+  bool IsOutsideSamples(monotonic_clock::time_point ta_base,
+                        double ta) const;
+
+  // Check whether the given timestamp lies after our current samples
+  bool IsAfterSamples(monotonic_clock::time_point ta_base, double ta) const;
+
+  std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>
+  GetReferenceTimestamp(monotonic_clock::time_point ta_base, double ta) const;
+
   // Returns the offset for the point in time, using the timestamps in the deque
   // to form a polyline used to interpolate.
   std::chrono::nanoseconds Offset(monotonic_clock::time_point ta) const;
@@ -380,26 +393,27 @@
       std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p1,
       monotonic_clock::time_point ta);
 
-  static std::chrono::nanoseconds InterpolateOffset(
+  static std::chrono::nanoseconds ExtrapolateOffset(
       std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
-      monotonic_clock::time_point /*ta*/) {
-    return std::get<1>(p0);
-  }
+      monotonic_clock::time_point ta);
 
   static std::chrono::nanoseconds InterpolateOffset(
       std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
       std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p1,
       monotonic_clock::time_point ta_base, double ta);
+
   static double InterpolateOffsetRemainder(
       std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> /*p0*/,
       std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> /*p1*/,
       monotonic_clock::time_point ta_base, double ta);
 
-  static std::chrono::nanoseconds InterpolateOffset(
+  static std::chrono::nanoseconds ExtrapolateOffset(
       std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
-      monotonic_clock::time_point /*ta_base*/, double /*ta*/) {
-    return std::get<1>(p0);
-  }
+      monotonic_clock::time_point /*ta_base*/, double /*ta*/);
+
+  static double ExtrapolateOffsetRemainder(
+      std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
+      monotonic_clock::time_point ta_base, double ta);
 
  private:
   // Removes the oldest timestamp.
diff --git a/aos/network/timestamp_filter_test.cc b/aos/network/timestamp_filter_test.cc
index 8ea4d57..caecccd 100644
--- a/aos/network/timestamp_filter_test.cc
+++ b/aos/network/timestamp_filter_test.cc
@@ -77,14 +77,14 @@
 }
 
 class FilterTest : public ::testing::Test {
-  public:
-   FlatbufferDetachedBuffer<Node> node_a_buffer =
-       JsonToFlatbuffer<Node>("{\"name\": \"test_a\"}");
-   const Node *const node_a = &node_a_buffer.message();
+ public:
+  FlatbufferDetachedBuffer<Node> node_a_buffer =
+      JsonToFlatbuffer<Node>("{\"name\": \"test_a\"}");
+  const Node *const node_a = &node_a_buffer.message();
 
-   FlatbufferDetachedBuffer<Node> node_b_buffer =
-       JsonToFlatbuffer<Node>("{\"name\": \"test_b\"}");
-   const Node *const node_b = &node_b_buffer.message();
+  FlatbufferDetachedBuffer<Node> node_b_buffer =
+      JsonToFlatbuffer<Node>("{\"name\": \"test_b\"}");
+  const Node *const node_b = &node_b_buffer.message();
 };
 
 using NoncausalTimestampFilterTest = FilterTest;
@@ -125,7 +125,6 @@
     filter.Sample(tb, ob);
     filter.Sample(tc, oc);
 
-
     EXPECT_TRUE(filter.Observe());
     EXPECT_EQ(*filter.Consume(), std::make_tuple(ta, oa));
 
@@ -145,7 +144,6 @@
     filter.Sample(tb, ob);
     filter.Sample(tc, oc);
 
-
     filter.Pop(tb);
     EXPECT_TRUE(filter.Observe());
     EXPECT_EQ(*filter.Consume(), std::make_tuple(tb, ob));
@@ -656,9 +654,9 @@
     ASSERT_EQ(filter.timestamps_size(), 2u);
     filter.FreezeUntil(tc);
 
-    EXPECT_DEATH(
-        { filter.Sample(tb, ob); },
-        "Tried to insert 0.100000000sec before 0.200000000sec, which is a frozen time");
+    EXPECT_DEATH({ filter.Sample(tb, ob); },
+                 "Tried to insert 0.100000000sec before 0.200000000sec, which "
+                 "is a frozen time");
   }
 
   {
@@ -672,9 +670,9 @@
     ASSERT_EQ(filter.timestamps_size(), 3u);
     filter.FreezeUntil(tb);
 
-    EXPECT_DEATH(
-        { filter.Sample(tb, oa); },
-        "Tried to insert 0.100000000sec before 0.100000000sec, which is a frozen time");
+    EXPECT_DEATH({ filter.Sample(tb, oa); },
+                 "Tried to insert 0.100000000sec before 0.100000000sec, which "
+                 "is a frozen time");
     EXPECT_DEATH({ filter.Sample(tb + chrono::nanoseconds(1), oa); },
                  "Can't pop an already frozen sample");
   }
@@ -684,11 +682,11 @@
 TEST_F(NoncausalTimestampFilterTest, InterpolateOffset) {
   const monotonic_clock::time_point e = monotonic_clock::epoch();
 
-  const monotonic_clock::time_point t1 = e + chrono::nanoseconds(0);
+  const monotonic_clock::time_point t1 = e + chrono::nanoseconds(10000);
   const chrono::nanoseconds o1 = chrono::nanoseconds(100);
   const double o1d = static_cast<double>(o1.count());
 
-  const monotonic_clock::time_point t2 = e + chrono::nanoseconds(1000);
+  const monotonic_clock::time_point t2 = t1 + chrono::nanoseconds(1000);
   const chrono::nanoseconds o2 = chrono::nanoseconds(150);
   const double o2d = static_cast<double>(o2.count());
 
@@ -714,73 +712,128 @@
 
   EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
                 std::make_tuple(t1, o1), std::make_tuple(t2, o2),
-                e + chrono::nanoseconds(500)),
+                t1 + chrono::nanoseconds(500)),
             chrono::nanoseconds(125));
   EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
                 std::make_tuple(t1, o1), std::make_tuple(t2, o2),
-                e + chrono::nanoseconds(500), 0.0),
+                t1 + chrono::nanoseconds(500), 0.0),
             o1);
   EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
                 std::make_tuple(t1, o1), std::make_tuple(t2, o2),
-                e + chrono::nanoseconds(500), 0.0),
+                t1 + chrono::nanoseconds(500), 0.0),
             25.);
 
   EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
                 std::make_tuple(t1, o1), std::make_tuple(t2, o2),
-                e + chrono::nanoseconds(-200)),
+                t1 + chrono::nanoseconds(-200)),
             chrono::nanoseconds(90));
   EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
-                std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, -200.),
+                std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, -200.),
             o1);
   EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
-                std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, -200.),
+                std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, -200.),
             -10.);
 
   EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
                 std::make_tuple(t1, o1), std::make_tuple(t2, o2),
-                e + chrono::nanoseconds(200)),
+                t1 + chrono::nanoseconds(200)),
             chrono::nanoseconds(110));
   EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
-                std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 200.),
+                std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, 200.),
             o1);
   EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
-                std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 200.),
+                std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, 200.),
             10.);
 
   EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
                 std::make_tuple(t1, o1), std::make_tuple(t2, o2),
-                e + chrono::nanoseconds(800)),
+                t1 + chrono::nanoseconds(800)),
             chrono::nanoseconds(140));
   EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
-                std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 800.),
+                std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, 800.),
             o1);
   EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
-                std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 800.),
+                std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, 800.),
             40.);
 
   EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
                 std::make_tuple(t1, o1), std::make_tuple(t2, o2),
-                e + chrono::nanoseconds(1200)),
+                t1 + chrono::nanoseconds(1200)),
             chrono::nanoseconds(160));
   EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
-                std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 1200.),
+                std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, 1200.),
             o1);
   EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
-                std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 1200.),
+                std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, 1200.),
             60.);
 
-  EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
-                std::make_tuple(t1, o1), e + chrono::nanoseconds(800)),
+  // Test extrapolation functions before t1 and after t2
+  EXPECT_EQ(
+      NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t1, o1), t1),
+      o1);
+
+  // With small offset (<1/kMaxVelocity() sec), low precision can't
+  // tell the difference since the delta is a fraction of a nanosecond
+  EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(
+                std::make_tuple(t1, o1), t1 + chrono::nanoseconds(-800)),
             o1);
-  EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(std::make_tuple(t1, o1),
-                                                        e, 800.),
+
+  EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(
+                std::make_tuple(t1, o1), e + chrono::nanoseconds(1000)),
+            o1 - chrono::nanoseconds(
+                     int64_t((t1 - (e + chrono::nanoseconds(1000))).count() *
+                             kMaxVelocity())));
+
+  EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(
+                std::make_tuple(t1, o1), t1 + chrono::nanoseconds(-9000)),
+            o1 + chrono::nanoseconds(int64_t(
+                     chrono::nanoseconds(-9000).count() * kMaxVelocity())));
+
+  // Test base + double version
+  EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t1, o1),
+                                                        e, 0.),
             o1);
-  EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
-                std::make_tuple(t1, o1), e + chrono::nanoseconds(-300)),
+  EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
+                std::make_tuple(t1, o1), e, 0.),
+            -(t1 - e).count() * kMaxVelocity());
+
+  EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t1, o1),
+                                                        t1, 0.),
             o1);
-  EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(std::make_tuple(t1, o1),
-                                                        e, -300.),
+  EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
+                std::make_tuple(t1, o1), t1, 0.),
+            0.);
+
+  EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t1, o1),
+                                                        t1, -1000.),
             o1);
+  EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
+                std::make_tuple(t1, o1), t1, -1000.),
+            -1000. * kMaxVelocity());
+
+  EXPECT_EQ(
+      NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t2, o2), t2),
+      o2);
+
+  EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t2, o2),
+                                                        t2, 0.0),
+            o2);
+  EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
+                std::make_tuple(t2, o2), t2, 0.0),
+            0.0);
+
+  // Test points past our last sample
+  EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(
+                std::make_tuple(t2, o2), t2 + chrono::nanoseconds(10000)),
+            chrono::nanoseconds(
+                static_cast<int64_t>(o2.count() - 10000. * kMaxVelocity())));
+
+  EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t2, o2),
+                                                        t2, 100.0),
+            o2);
+  EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
+                std::make_tuple(t2, o2), t2, 100.0),
+            -100.0 * kMaxVelocity());
 }
 
 // Tests that FindTimestamps finds timestamps in a sequence.
@@ -856,20 +909,22 @@
 // and FindTimestamps correctly.
 TEST_F(NoncausalTimestampFilterTest, Offset) {
   const monotonic_clock::time_point e = monotonic_clock::epoch();
-  // Note: t1, t2, t3 need to be picked such that the slop is small so filter
+  // Note: t1, t2, t3 need to be picked such that the slope is small so filter
   // doesn't modify the timestamps.
-  const monotonic_clock::time_point t1 = e + chrono::nanoseconds(0);
+  const monotonic_clock::time_point t1 = e + chrono::nanoseconds(1000);
   const chrono::nanoseconds o1 = chrono::nanoseconds(100);
   const double o1d = static_cast<double>(o1.count());
 
-  const monotonic_clock::time_point t2 = e + chrono::microseconds(1000);
+  const monotonic_clock::time_point t2 = e + chrono::microseconds(2000);
   const chrono::nanoseconds o2 = chrono::nanoseconds(150);
   const double o2d = static_cast<double>(o2.count());
 
-  const monotonic_clock::time_point t3 = e + chrono::microseconds(2000);
+  const monotonic_clock::time_point t3 = e + chrono::microseconds(3000);
   const chrono::nanoseconds o3 = chrono::nanoseconds(50);
   const double o3d = static_cast<double>(o3.count());
 
+  const monotonic_clock::time_point t4 = e + chrono::microseconds(4000);
+
   NoncausalTimestampFilter filter(node_a, node_b);
 
   filter.Sample(t1, o1);
@@ -877,7 +932,17 @@
   // 1 point is handled properly.
   EXPECT_EQ(filter.Offset(t1), o1);
   EXPECT_EQ(filter.Offset(t1, 0.0), std::make_pair(o1, 0.0));
-  EXPECT_EQ(filter.Offset(t2, 0.0), std::make_pair(o1, 0.0));
+  // Check if we ask for something away from point that we get an offset
+  // based on the MaxVelocity allowed
+  const double offset_pre = -(t1 - e).count() * kMaxVelocity();
+  EXPECT_EQ(filter.Offset(e),
+            o1 + chrono::nanoseconds(static_cast<int64_t>(offset_pre)));
+  EXPECT_EQ(filter.Offset(e, 0.0), std::make_pair(o1, offset_pre));
+
+  double offset_post = -(t2 - t1).count() * kMaxVelocity();
+  EXPECT_EQ(filter.Offset(t2),
+            o1 + chrono::nanoseconds(static_cast<int64_t>(offset_post)));
+  EXPECT_EQ(filter.Offset(t2, 0.0), std::make_pair(o1, offset_post));
 
   filter.Sample(t2, o2);
   filter.Sample(t3, o3);
@@ -898,7 +963,18 @@
                 e + (t2.time_since_epoch() + t3.time_since_epoch()) / 2, 0.),
             std::make_pair(o2, (o2d + o3d) / 2. - o2d));
 
-  EXPECT_EQ(filter.Offset(t3, 0.0), std::make_pair(o2, o3d - o2d));
+  EXPECT_EQ(filter.Offset(t3, 0.0), std::make_pair(o3, 0.0));
+
+  // Check that we still get same answer for times before our sample data...
+  EXPECT_EQ(filter.Offset(e),
+            o1 + chrono::nanoseconds(static_cast<int64_t>(offset_pre)));
+  EXPECT_EQ(filter.Offset(e, 0.0), std::make_pair(o1, offset_pre));
+  // ... and after
+  offset_post = -(t4 - t3).count() * kMaxVelocity();
+  EXPECT_EQ(
+      filter.Offset(t4).count(),
+      (o3 + chrono::nanoseconds(static_cast<int64_t>(offset_post))).count());
+  EXPECT_EQ(filter.Offset(t4, 0.0), std::make_pair(o3, offset_post));
 }
 
 // Tests that the cost function handles a single point line properly, and the
@@ -917,17 +993,33 @@
   filter.Sample(t1, o1);
 
   // Spot check some known points.
+  // NOTE that double arg must be 0 <= ta < 1. for Offset and OffsetError
+  // We can handle values outside that range for Cost functions
+  EXPECT_EQ(filter.Offset(t1, 0.0), std::make_pair(o1, 0.0));
+  EXPECT_EQ(filter.Offset(t1, 0.5), std::make_pair(o1, -0.5 * kMaxVelocity()));
   EXPECT_EQ(filter.OffsetError(t1, 0.0, t1 + o1, 0.0), 0.0);
-  EXPECT_EQ(filter.OffsetError(t1, 5.0, t1 + o1, 5.0), 0.0);
+  EXPECT_EQ(filter.OffsetError(t1, 0.5, t1 + o1, 0.5), 0.5 * kMaxVelocity());
+  EXPECT_EQ(filter.OffsetError(t1, 0.5, t1 + o1, 0.0),
+            -0.5 + 0.5 * kMaxVelocity());
+
   EXPECT_EQ(filter.Cost(t1, 0.0, t1 + o1, 0.0), 0.0);
-  EXPECT_EQ(filter.Cost(t1, 1.0, t1 + o1, 0.0), 1.0);
+  EXPECT_EQ(filter.Cost(t1, 1.0, t1 + o1, 1.0),
+            std::pow(-1.0 * kMaxVelocity(), 2.0));
+  EXPECT_EQ(filter.Cost(t1, 1.0, t1 + o1, 0.0),
+            std::pow(1.0 - kMaxVelocity(), 2.0));
+  EXPECT_EQ(filter.Cost(t1, -0.5, t1 + o1, 0.0),
+            std::pow(0.5 + 0.5 * kMaxVelocity(), 2.0));
+  EXPECT_EQ(filter.Cost(t1, 0.5, t1 + o1, 0.0),
+            std::pow(-0.5 + 0.5 * kMaxVelocity(), 2.0));
 
   constexpr double kDelta = 10.;
 
   // Perturb ta and tb so we make sure it works away from 0.
-  for (double ta_nominal : {-1000.0, 0.0, 1000.0}) {
-    for (double tb_nominal : {-2000.0, 0.0, 2000.0}) {
+  // Avoid offsets of 0.0, since this function isn't well behaved there
+  for (double ta_nominal : {-1000.0, 20.0, 1000.0}) {
+    for (double tb_nominal : {-2000.0, 20.0, 2000.0}) {
       {
+        // Evaluate functions around (ta (or tb) + kDelta)
         const double minus_costa =
             filter.Cost(t1, ta_nominal - kDelta, t1 + o1, tb_nominal);
         const double plus_costa =
@@ -974,7 +1066,7 @@
 
   // Spot check some known points.
   EXPECT_EQ(filter.OffsetError(t1, 0.0, t1 + o1, 0.0), 0.0);
-  EXPECT_EQ(filter.OffsetError(t1, 5.0, t1 + o1, 5.0), -0.0025);
+  EXPECT_EQ(filter.OffsetError(t1, 0.5, t1 + o1, 0.5), -0.00025);
   EXPECT_EQ(filter.OffsetError(t2, 0.0, t2 + o2, 0.0), 0.0);
   EXPECT_EQ(filter.OffsetError(t3, 0.0, t3 + o3, 0.0), 0.0);
 
@@ -985,10 +1077,10 @@
   // Perturb ta and tbd so we make sure it works away from 0.
   constexpr double kDelta = 10.;
 
-  // Note: don't test 0 offset because that makes the computed slope at t2
+  // Note: don't test 0 delta because that makes the computed slope at t2
   // wrong.
   for (double ta_nominal : {-1000.0, 20.0, 1000.0}) {
-    for (double tb_nominal : {-2000.0, 20.0, 2000.0}) {
+    for (double tb_nominal : {-4000.0, -2000.0, 20.0, 2000.0, 4000.0}) {
       // Check points round each of the 3 points in the polyline.  Use 3 points
       // so if we mess up the point selection code, it shows up.
       {