diff --git a/src/image/p_median3x3.c b/src/image/p_median3x3.c
index 28437b2..7c6fa8d 100644
--- a/src/image/p_median3x3.c
+++ b/src/image/p_median3x3.c
@@ -1,5 +1,45 @@
+#include <string.h>
 #include <pal.h>
 
+inline void sort(float *ptr, int lhs, int rhs)
+{	
+    if (ptr[lhs]>ptr[rhs]) {
+        float tmp = ptr[rhs];
+        ptr[rhs] = ptr[lhs];
+        ptr[lhs] = tmp;
+    }
+}
+
+// median of a 9 element vector
+void median9(const float *a, float *c, int p, p_team_t team)
+{
+    float copy_a[9];
+    memcpy(copy_a, a, sizeof(float) * 9);
+    
+    // minimum number of comparisons for median of 9 elements
+    sort(copy_a, 1, 2);
+    sort(copy_a, 4, 5);
+    sort(copy_a, 7, 8);
+    sort(copy_a, 0, 1);
+    sort(copy_a, 3, 4);
+    sort(copy_a, 6, 7);
+    sort(copy_a, 1, 2);
+    sort(copy_a, 4, 5);
+    sort(copy_a, 7, 8);
+    sort(copy_a, 0, 3);
+    sort(copy_a, 5, 8);
+    sort(copy_a, 4, 7);
+    sort(copy_a, 3, 6);
+    sort(copy_a, 1, 4);
+    sort(copy_a, 2, 5);
+    sort(copy_a, 3, 7);
+    sort(copy_a, 4, 2);
+    sort(copy_a, 6, 4);
+    sort(copy_a, 4, 2);
+
+    *c = copy_a[4];
+}
+
 /*
  * A median 3x3 filter.
  *
@@ -44,7 +84,7 @@ void p_median3x3_f32(const float *x, float *r, int rows, int cols,
         buffer[7] = *(px + cols + cols + 1);
         buffer[8] = *(px + cols + cols + 2);
 
-        p_median_f32(buffer, pr, 9, 0, 0);
+        median9(buffer, pr, 0, 0);
         pr++;
         px += 3;
         // other windows differ only by one column
@@ -55,7 +95,7 @@ void p_median3x3_f32(const float *x, float *r, int rows, int cols,
             buffer[buffer_col + 3] = *(px + cols);
             buffer[buffer_col + 6] = *(px + cols + cols);
 
-            p_median_f32(buffer, pr, 9, 0, 0);
+            median9(buffer, pr, 0, 0);
             pr++;
             px++;
         }