diff --git a/wpiutil/src/main/java/edu/wpi/first/util/InterpolatingTreeMap.java b/wpiutil/src/main/java/edu/wpi/first/util/InterpolatingTreeMap.java
new file mode 100644
index 0000000000..ade2e5a42d
--- /dev/null
+++ b/wpiutil/src/main/java/edu/wpi/first/util/InterpolatingTreeMap.java
@@ -0,0 +1,96 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util;
+
+import java.util.TreeMap;
+
+/**
+ * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
+ * from points that are defined. This uses linear interpolation.
+ */
+public class InterpolatingTreeMap<K extends Number, V extends Number> {
+  private final TreeMap<K, V> m_map = new TreeMap<>();
+
+  /**
+   * Inserts a key-value pair.
+   *
+   * @param key The key.
+   * @param value The value.
+   */
+  public void put(K key, V value) {
+    m_map.put(key, value);
+  }
+
+  /**
+   * Returns the value associated with a given key.
+   *
+   * <p>If there's no matching key, the value returned will be a linear interpolation between the
+   * keys before and after the provided one.
+   *
+   * @param key The key.
+   * @return The value associated with the given key.
+   */
+  public Double get(K key) {
+    V val = m_map.get(key);
+    if (val == null) {
+      K ceilingKey = m_map.ceilingKey(key);
+      K floorKey = m_map.floorKey(key);
+
+      if (ceilingKey == null && floorKey == null) {
+        return null;
+      }
+      if (ceilingKey == null) {
+        return m_map.get(floorKey).doubleValue();
+      }
+      if (floorKey == null) {
+        return m_map.get(ceilingKey).doubleValue();
+      }
+      V floor = m_map.get(floorKey);
+      V ceiling = m_map.get(ceilingKey);
+
+      return interpolate(floor, ceiling, inverseInterpolate(ceilingKey, key, floorKey));
+    } else {
+      return val.doubleValue();
+    }
+  }
+
+  /** Clears the contents. */
+  public void clear() {
+    m_map.clear();
+  }
+
+  /**
+   * Return the value interpolated between val1 and val2 by the interpolant d.
+   *
+   * @param val1 The lower part of the interpolation range.
+   * @param val2 The upper part of the interpolation range.
+   * @param d The interpolant in the range [0, 1].
+   * @return The interpolated value.
+   */
+  private double interpolate(V val1, V val2, double d) {
+    double dydx = val2.doubleValue() - val1.doubleValue();
+    return dydx * d + val1.doubleValue();
+  }
+
+  /**
+   * Return where within interpolation range [0, 1] q is between down and up.
+   *
+   * @param up Upper part of interpolation range.
+   * @param q Query.
+   * @param down Lower part of interpolation range.
+   * @return Interpolant in range [0, 1].
+   */
+  private double inverseInterpolate(K up, K q, K down) {
+    double upperToLower = up.doubleValue() - down.doubleValue();
+    if (upperToLower <= 0) {
+      return 0.0;
+    }
+    double queryToLower = q.doubleValue() - down.doubleValue();
+    if (queryToLower <= 0) {
+      return 0.0;
+    }
+    return queryToLower / upperToLower;
+  }
+}
diff --git a/wpiutil/src/main/native/include/wpi/interpolating_map.h b/wpiutil/src/main/native/include/wpi/interpolating_map.h
new file mode 100644
index 0000000000..5eff514c19
--- /dev/null
+++ b/wpiutil/src/main/native/include/wpi/interpolating_map.h
@@ -0,0 +1,87 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <map>
+#include <utility>
+
+namespace wpi {
+
+/**
+ * Implements a table of key-value pairs with linear interpolation between
+ * values.
+ *
+ * If there's no matching key, the value returned will be a linear interpolation
+ * between the keys before and after the provided one.
+ *
+ * @tparam Key   The key type.
+ * @tparam Value The value type.
+ */
+template <typename Key, typename Value>
+class interpolating_map {
+ public:
+  /**
+   * Inserts a key-value pair.
+   *
+   * @param key   The key.
+   * @param value The value.
+   */
+  void insert(const Key& key, const Value& value) {
+    m_container.insert(std::make_pair(key, value));
+  }
+
+  /**
+   * Inserts a key-value pair.
+   *
+   * @param key   The key.
+   * @param value The value.
+   */
+  void insert(Key&& key, Value&& value) {
+    m_container.insert(std::make_pair(key, value));
+  }
+
+  /**
+   * Returns the value associated with a given key.
+   *
+   * If there's no matching key, the value returned will be a linear
+   * interpolation between the keys before and after the provided one.
+   *
+   * @param key The key.
+   */
+  Value operator[](const Key& key) const {
+    using const_iterator = typename std::map<Key, Value>::const_iterator;
+
+    // Get iterator to upper bound key-value pair for the given key
+    const_iterator upper = m_container.upper_bound(key);
+
+    // If key > largest key in table, return value for largest table key
+    if (upper == m_container.end()) {
+      return (--upper)->second;
+    }
+
+    // If key <= smallest key in table, return value for smallest table key
+    if (upper == m_container.begin()) {
+      return upper->second;
+    }
+
+    // Get iterator to lower bound key-value pair
+    const_iterator lower = upper;
+    --lower;
+
+    // Perform linear interpolation between lower and upper bound
+    const double delta = (key - lower->first) / (upper->first - lower->first);
+    return delta * upper->second + (1.0 - delta) * lower->second;
+  }
+
+  /**
+   * Clears the contents.
+   */
+  void clear() { m_container.clear(); }
+
+ private:
+  std::map<Key, Value> m_container;
+};
+
+}  // namespace wpi
diff --git a/wpiutil/src/test/java/edu/wpi/first/util/InterpolatingTreeMapTest.java b/wpiutil/src/test/java/edu/wpi/first/util/InterpolatingTreeMapTest.java
new file mode 100644
index 0000000000..42b87bcb57
--- /dev/null
+++ b/wpiutil/src/test/java/edu/wpi/first/util/InterpolatingTreeMapTest.java
@@ -0,0 +1,134 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class InterpolatingTreeMapTest {
+  @Test
+  void testInterpolationDouble() {
+    InterpolatingTreeMap<Double, Double> table = new InterpolatingTreeMap<>();
+
+    table.put(125.0, 450.0);
+    table.put(200.0, 510.0);
+    table.put(268.0, 525.0);
+    table.put(312.0, 550.0);
+    table.put(326.0, 650.0);
+
+    // Key below minimum gives smallest value
+    assertEquals(450.0, table.get(100.0));
+
+    // Minimum key gives exact value
+    assertEquals(450.0, table.get(125.0));
+
+    // Key gives interpolated value
+    assertEquals(480.0, table.get(162.5));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(510.0, table.get(200.0));
+
+    // Maximum key gives exact value
+    assertEquals(650.0, table.get(326.0));
+
+    // Key above maximum gives largest value
+    assertEquals(650.0, table.get(400.0));
+  }
+
+  @Test
+  void testInterpolationClear() {
+    InterpolatingTreeMap<Double, Double> table = new InterpolatingTreeMap<>();
+
+    table.put(125.0, 450.0);
+    table.put(200.0, 510.0);
+    table.put(268.0, 525.0);
+    table.put(312.0, 550.0);
+    table.put(326.0, 650.0);
+
+    // Key below minimum gives smallest value
+    assertEquals(450.0, table.get(100.0));
+
+    // Minimum key gives exact value
+    assertEquals(450.0, table.get(125.0));
+
+    // Key gives interpolated value
+    assertEquals(480.0, table.get(162.5));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(510.0, table.get(200.0));
+
+    // Maximum key gives exact value
+    assertEquals(650.0, table.get(326.0));
+
+    // Key above maximum gives largest value
+    assertEquals(650.0, table.get(400.0));
+
+    table.clear();
+
+    table.put(100.0, 250.0);
+    table.put(200.0, 500.0);
+
+    assertEquals(375.0, table.get(150.0));
+  }
+
+  @Test
+  void testInterpolationInteger() {
+    InterpolatingTreeMap<Integer, Integer> table = new InterpolatingTreeMap<>();
+
+    table.put(125, 450);
+    table.put(200, 510);
+    table.put(268, 525);
+    table.put(312, 550);
+    table.put(326, 650);
+
+    // Key below minimum gives smallest value
+    assertEquals(450.0, table.get(100));
+
+    // Minimum key gives exact value
+    assertEquals(450.0, table.get(125));
+
+    // Key gives interpolated value
+    assertEquals(479.6, table.get(162));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(510.0, table.get(200));
+
+    // Maximum key gives exact value
+    assertEquals(650.0, table.get(326));
+
+    // Key above maximum gives largest value
+    assertEquals(650.0, table.get(400));
+  }
+
+  @Test
+  void testInterpolationLong() {
+    InterpolatingTreeMap<Long, Long> table = new InterpolatingTreeMap<>();
+
+    table.put(125L, 450L);
+    table.put(200L, 510L);
+    table.put(268L, 525L);
+    table.put(312L, 550L);
+    table.put(326L, 650L);
+
+    // Key below minimum gives smallest value
+    assertEquals(450.0, table.get(100L));
+
+    // Minimum key gives exact value
+    assertEquals(450.0, table.get(125L));
+
+    // Key gives interpolated value
+    assertEquals(479.6, table.get(162L));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(510.0, table.get(200L));
+
+    // Maximum key gives exact value
+    assertEquals(650.0, table.get(326L));
+
+    // Key above maximum gives largest value
+    assertEquals(650.0, table.get(400L));
+  }
+}
diff --git a/wpiutil/src/test/native/cpp/InterpolatingMapTest.cpp b/wpiutil/src/test/native/cpp/InterpolatingMapTest.cpp
new file mode 100644
index 0000000000..4479da5914
--- /dev/null
+++ b/wpiutil/src/test/native/cpp/InterpolatingMapTest.cpp
@@ -0,0 +1,52 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/interpolating_map.h"  // NOLINT(build/include_order)
+
+#include <gtest/gtest.h>
+
+TEST(InterpolatingMapTest, Insert) {
+  wpi::interpolating_map<double, double> table;
+
+  table.insert(125, 450);
+  table.insert(200, 510);
+  table.insert(268, 525);
+  table.insert(312, 550);
+  table.insert(326, 650);
+
+  // Key below minimum gives smallest value
+  EXPECT_EQ(450, table[100]);
+
+  // Minimum key gives exact value
+  EXPECT_EQ(450, table[125]);
+
+  // Key gives interpolated value
+  EXPECT_EQ(480, table[162.5]);
+
+  // Key at right of interpolation range gives exact value
+  EXPECT_EQ(510, table[200]);
+
+  // Maximum key gives exact value
+  EXPECT_EQ(650, table[326]);
+
+  // Key above maximum gives largest value
+  EXPECT_EQ(650, table[400]);
+}
+
+TEST(InterpolatingMapTest, Clear) {
+  wpi::interpolating_map<double, double> table;
+
+  table.insert(125, 450);
+  table.insert(200, 510);
+  table.insert(268, 525);
+  table.insert(312, 550);
+  table.insert(326, 650);
+
+  table.clear();
+
+  table.insert(100, 250);
+  table.insert(200, 500);
+
+  EXPECT_EQ(375, table[150]);
+}