diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
index 95ed5bfe25..9e74aba762 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
@@ -145,4 +145,47 @@ public final class MathUtil {
   public static double interpolate(double startValue, double endValue, double t) {
     return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1);
   }
+
+  /**
+   * Checks if the given value matches an expected value within a certain tolerance.
+   *
+   * @param expected The expected value
+   * @param actual The actual value
+   * @param tolerance The allowed difference between the actual and the expected value
+   * @return Whether or not the actual value is within the allowed tolerance
+   */
+  public static boolean isNear(double expected, double actual, double tolerance) {
+    if (tolerance < 0) {
+      throw new IllegalArgumentException("Tolerance must be a non-negative number!");
+    }
+    return Math.abs(expected - actual) < tolerance;
+  }
+
+  /**
+   * Checks if the given value matches an expected value within a certain tolerance. Supports
+   * continuous input for cases like absolute encoders.
+   *
+   * <p>Continuous input means that the min and max value are considered to be the same point, and
+   * tolerances can be checked across them. A common example would be for absolute encoders: calling
+   * isNear(2, 359, 5, 0, 360) returns true because 359 is 1 away from 360 (which is treated as the
+   * same as 0) and 2 is 2 away from 0, adding up to an error of 3 degrees, which is within the
+   * given tolerance of 5.
+   *
+   * @param expected The expected value
+   * @param actual The actual value
+   * @param tolerance The allowed difference between the actual and the expected value
+   * @param min Smallest value before wrapping around to the largest value
+   * @param max Largest value before wrapping around to the smallest value
+   * @return Whether or not the actual value is within the allowed tolerance
+   */
+  public static boolean isNear(
+      double expected, double actual, double tolerance, double min, double max) {
+    if (tolerance < 0) {
+      throw new IllegalArgumentException("Tolerance must be a non-negative number!");
+    }
+    // Max error is exactly halfway between the min and max
+    double errorBound = (max - min) / 2.0;
+    double error = MathUtil.inputModulus(expected - actual, -errorBound, errorBound);
+    return Math.abs(error) < tolerance;
+  }
 }
diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h
index 7947061a81..26f106ef6b 100644
--- a/wpimath/src/main/native/include/frc/MathUtil.h
+++ b/wpimath/src/main/native/include/frc/MathUtil.h
@@ -105,6 +105,58 @@ constexpr T InputModulus(T input, T minimumInput, T maximumInput) {
   return input;
 }
 
+/**
+ * Checks if the given value matches an expected value within a certain
+ * tolerance.
+ *
+ * @param expected The expected value
+ * @param actual The actual value
+ * @param tolerance The allowed difference between the actual and the expected
+ * value
+ * @return Whether or not the actual value is within the allowed tolerance
+ */
+template <typename T>
+  requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
+constexpr bool IsNear(T expected, T actual, T tolerance) {
+  if constexpr (std::is_arithmetic_v<T>) {
+    return std::abs(expected - actual) < tolerance;
+  } else {
+    return units::math::abs(expected - actual) < tolerance;
+  }
+}
+
+/**
+ * Checks if the given value matches an expected value within a certain
+ * tolerance. Supports continuous input for cases like absolute encoders.
+ *
+ * Continuous input means that the min and max value are considered to be the
+ * same point, and tolerances can be checked across them. A common example
+ * would be for absolute encoders: calling isNear(2, 359, 5, 0, 360) returns
+ * true because 359 is 1 away from 360 (which is treated as the same as 0) and
+ * 2 is 2 away from 0, adding up to an error of 3 degrees, which is within the
+ * given tolerance of 5.
+ *
+ * @param expected The expected value
+ * @param actual The actual value
+ * @param tolerance The allowed difference between the actual and the expected
+ * value
+ * @param min Smallest value before wrapping around to the largest value
+ * @param max Largest value before wrapping around to the smallest value
+ * @return Whether or not the actual value is within the allowed tolerance
+ */
+template <typename T>
+  requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
+constexpr bool IsNear(T expected, T actual, T tolerance, T min, T max) {
+  T errorBound = (max - min) / 2.0;
+  T error = frc::InputModulus<T>(expected - actual, -errorBound, errorBound);
+
+  if constexpr (std::is_arithmetic_v<T>) {
+    return std::abs(error) < tolerance;
+  } else {
+    return units::math::abs(error) < tolerance;
+  }
+}
+
 /**
  * Wraps an angle to the range -pi to pi radians (-180 to 180 degrees).
  *
diff --git a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
index 5e1a01e57e..f5764773e6 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
@@ -5,6 +5,8 @@
 package edu.wpi.first.math;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.wpilibj.UtilityClassTest;
 import org.junit.jupiter.api.Test;
@@ -93,4 +95,50 @@ class MathUtilTest extends UtilityClassTest<MathUtil> {
     assertEquals(MathUtil.angleModulus(Math.PI / 2), Math.PI / 2);
     assertEquals(MathUtil.angleModulus(-Math.PI / 2), -Math.PI / 2);
   }
+
+  @Test
+  void testIsNear() {
+    // The answer is always 42
+    // Positive integer checks
+    assertTrue(MathUtil.isNear(42, 42, 1));
+    assertTrue(MathUtil.isNear(42, 41, 2));
+    assertTrue(MathUtil.isNear(42, 43, 2));
+    assertFalse(MathUtil.isNear(42, 44, 1));
+
+    // Negative integer checks
+    assertTrue(MathUtil.isNear(-42, -42, 1));
+    assertTrue(MathUtil.isNear(-42, -41, 2));
+    assertTrue(MathUtil.isNear(-42, -43, 2));
+    assertFalse(MathUtil.isNear(-42, -44, 1));
+
+    // Mixed sign integer checks
+    assertFalse(MathUtil.isNear(-42, 42, 1));
+    assertFalse(MathUtil.isNear(-42, 41, 2));
+    assertFalse(MathUtil.isNear(-42, 43, 2));
+    assertFalse(MathUtil.isNear(42, -42, 1));
+    assertFalse(MathUtil.isNear(42, -41, 2));
+    assertFalse(MathUtil.isNear(42, -43, 2));
+
+    // Floating point checks
+    assertTrue(MathUtil.isNear(42, 41.5, 1));
+    assertTrue(MathUtil.isNear(42, 42.5, 1));
+    assertTrue(MathUtil.isNear(42, 41.5, 0.75));
+    assertTrue(MathUtil.isNear(42, 42.5, 0.75));
+
+    // Wraparound checks
+    assertTrue(MathUtil.isNear(0, 356, 5, 0, 360));
+    assertTrue(MathUtil.isNear(0, -356, 5, 0, 360));
+    assertTrue(MathUtil.isNear(0, 4, 5, 0, 360));
+    assertTrue(MathUtil.isNear(0, -4, 5, 0, 360));
+    assertTrue(MathUtil.isNear(400, 41, 5, 0, 360));
+    assertTrue(MathUtil.isNear(400, -319, 5, 0, 360));
+    assertTrue(MathUtil.isNear(400, 401, 5, 0, 360));
+    assertFalse(MathUtil.isNear(0, 356, 2.5, 0, 360));
+    assertFalse(MathUtil.isNear(0, -356, 2.5, 0, 360));
+    assertFalse(MathUtil.isNear(0, 4, 2.5, 0, 360));
+    assertFalse(MathUtil.isNear(0, -4, 2.5, 0, 360));
+    assertFalse(MathUtil.isNear(400, 35, 5, 0, 360));
+    assertFalse(MathUtil.isNear(400, -315, 5, 0, 360));
+    assertFalse(MathUtil.isNear(400, 395, 5, 0, 360));
+  }
 }
diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp
index a836a77ca9..54ba5b2453 100644
--- a/wpimath/src/test/native/cpp/MathUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp
@@ -117,3 +117,49 @@ TEST(MathUtilTest, AngleModulus) {
   EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{-std::numbers::pi / 2}),
                   units::radian_t{-std::numbers::pi / 2});
 }
+
+TEST(MathUtilTest, IsNear) {
+  // The answer is always 42
+  // Positive integer checks
+  EXPECT_TRUE(frc::IsNear(42, 42, 1));
+  EXPECT_TRUE(frc::IsNear(42, 41, 2));
+  EXPECT_TRUE(frc::IsNear(42, 43, 2));
+  EXPECT_FALSE(frc::IsNear(42, 44, 1));
+
+  // Negative integer checks
+  EXPECT_TRUE(frc::IsNear(-42, -42, 1));
+  EXPECT_TRUE(frc::IsNear(-42, -41, 2));
+  EXPECT_TRUE(frc::IsNear(-42, -43, 2));
+  EXPECT_FALSE(frc::IsNear(-42, -44, 1));
+
+  // Mixed sign integer checks
+  EXPECT_FALSE(frc::IsNear(-42, 42, 1));
+  EXPECT_FALSE(frc::IsNear(-42, 41, 2));
+  EXPECT_FALSE(frc::IsNear(-42, 43, 2));
+  EXPECT_FALSE(frc::IsNear(42, -42, 1));
+  EXPECT_FALSE(frc::IsNear(42, -41, 2));
+  EXPECT_FALSE(frc::IsNear(42, -43, 2));
+
+  // Floating point checks
+  EXPECT_TRUE(frc::IsNear<double>(42, 41.5, 1));
+  EXPECT_TRUE(frc::IsNear<double>(42, 42.5, 1));
+  EXPECT_TRUE(frc::IsNear<double>(42, 41.5, 0.75));
+  EXPECT_TRUE(frc::IsNear<double>(42, 42.5, 0.75));
+
+  // Wraparound checks
+  EXPECT_TRUE(frc::IsNear(0_deg, 356_deg, 5_deg, 0_deg, 360_deg));
+  EXPECT_TRUE(frc::IsNear(0, -356, 5, 0, 360));
+  EXPECT_TRUE(frc::IsNear(0, 4, 5, 0, 360));
+  EXPECT_TRUE(frc::IsNear(0, -4, 5, 0, 360));
+  EXPECT_TRUE(frc::IsNear(400, 41, 5, 0, 360));
+  EXPECT_TRUE(frc::IsNear(400, -319, 5, 0, 360));
+  EXPECT_TRUE(frc::IsNear(400, 401, 5, 0, 360));
+  EXPECT_FALSE(frc::IsNear<double>(0, 356, 2.5, 0, 360));
+  EXPECT_FALSE(frc::IsNear<double>(0, -356, 2.5, 0, 360));
+  EXPECT_FALSE(frc::IsNear<double>(0, 4, 2.5, 0, 360));
+  EXPECT_FALSE(frc::IsNear<double>(0, -4, 2.5, 0, 360));
+  EXPECT_FALSE(frc::IsNear(400, 35, 5, 0, 360));
+  EXPECT_FALSE(frc::IsNear(400, -315, 5, 0, 360));
+  EXPECT_FALSE(frc::IsNear(400, 395, 5, 0, 360));
+  EXPECT_FALSE(frc::IsNear(0_deg, -4_deg, 2.5_deg, 0_deg, 360_deg));
+}