diff --git a/src/lib/mathlib/math/filter/MedianFilter.hpp b/src/lib/mathlib/math/filter/MedianFilter.hpp
index b483e59e7c..3fcdc800c3 100644
--- a/src/lib/mathlib/math/filter/MedianFilter.hpp
+++ b/src/lib/mathlib/math/filter/MedianFilter.hpp
@@ -48,6 +48,11 @@
 namespace math
 {
 
+template<typename U> struct is_floating_point { static constexpr bool value = false; };
+template<> struct is_floating_point<float> { static constexpr bool value = true; };
+template<> struct is_floating_point<double> { static constexpr bool value = true; };
+template<> struct is_floating_point<long double> { static constexpr bool value = true; };
+
 template<typename T, int WINDOW = 3>
 class MedianFilter
 {
@@ -82,7 +87,30 @@ private:
 
 	static int cmp(const void *a, const void *b)
 	{
-		return (*(T *)a >= *(T *)b) ? 1 : -1;
+		const T va = *(const T *)a;
+		const T vb = *(const T *)b;
+
+		if constexpr(is_floating_point<T>::value) {
+			if (!__builtin_isfinite(va) && !__builtin_isfinite(vb)) {
+				return 0;
+
+			} else if (!__builtin_isfinite(va)) {
+				return  1;
+
+			} else if (!__builtin_isfinite(vb)) {
+				return -1;
+			}
+		}
+
+		if (va < vb) {
+			return -1;
+		}
+
+		if (va > vb) {
+			return  1;
+		}
+
+		return 0;
 	}
 
 	T _buffer[WINDOW] {};
diff --git a/src/lib/mathlib/math/test/MedianFilterTest.cpp b/src/lib/mathlib/math/test/MedianFilterTest.cpp
index f74cc7ef76..2cbafc26b8 100644
--- a/src/lib/mathlib/math/test/MedianFilterTest.cpp
+++ b/src/lib/mathlib/math/test/MedianFilterTest.cpp
@@ -91,3 +91,54 @@ TEST_F(MedianFilterTest, test5i_100)
 		EXPECT_EQ(median_filter5.apply(i), max(0, i - 2));
 	}
 }
+
+TEST_F(MedianFilterTest, test5f_nan_majority_finite)
+{
+	MedianFilter<float, 5> median_filter5;
+	median_filter5.insert(1.0f);
+	median_filter5.insert(2.0f);
+	median_filter5.insert(NAN);
+	median_filter5.insert(3.0f);
+	median_filter5.insert(NAN);
+	EXPECT_TRUE(PX4_ISFINITE(median_filter5.median()));
+	EXPECT_EQ(median_filter5.median(), 3.0f);
+}
+
+TEST_F(MedianFilterTest, test5f_nan_majority_nan)
+{
+	MedianFilter<float, 5> median_filter5;
+	median_filter5.insert(NAN);
+	median_filter5.insert(NAN);
+	median_filter5.insert(1.0f);
+	median_filter5.insert(NAN);
+	median_filter5.insert(2.0f);
+	EXPECT_FALSE(PX4_ISFINITE(median_filter5.median()));
+}
+
+TEST_F(MedianFilterTest, test3f_equal_values)
+{
+	MedianFilter<float, 3> median_filter3;
+	median_filter3.insert(5.0f);
+	median_filter3.insert(5.0f);
+	median_filter3.insert(5.0f);
+	EXPECT_EQ(median_filter3.median(), 5.0f);
+}
+
+TEST_F(MedianFilterTest, test3f_nan_spike_recovery)
+{
+	MedianFilter<float, 3> median_filter3;
+
+	// Fill with finite values
+	median_filter3.apply(1.0f);
+	median_filter3.apply(2.0f);
+	EXPECT_EQ(median_filter3.apply(3.0f), 2.0f); // window: [1, 2, 3]
+
+	// NaN spike enters — majority still finite
+	EXPECT_TRUE(PX4_ISFINITE(median_filter3.apply(NAN))); // window: [2, 3, NaN]
+	EXPECT_EQ(median_filter3.median(), 3.0f);
+
+	// Recovery — NaN leaves the window
+	EXPECT_EQ(median_filter3.apply(4.0f), 4.0f); // window: [3, NaN, 4] → sorted [3, 4, NaN] → median 4
+	EXPECT_EQ(median_filter3.apply(5.0f), 5.0f); // window: [NaN, 4, 5] → sorted [4, 5, NaN] → median 5
+	EXPECT_EQ(median_filter3.apply(6.0f), 5.0f); // window: [4, 5, 6] → all finite again
+}