How to build a median filter function?
here is code for a "rolling median" copied from the reference of the reference i made above.  i believe the computational cost of the alg (per sample) is $O(\log_2(N))$.

//Copyright (c) 2011 ashelly.myopenid.com under &lt;http://w...content-available-to-author-only...e.org/licenses/mit-license&gt;

#include &lt;stdlib.h&gt;

//Customize for your data Item type
typedef int Item;
#define ItemLess(a,b)  ((a)&lt;(b))
#define ItemMean(a,b)  (((a)+(b))/2)

typedef struct Mediator_t
{
   Item* data;  //circular queue of values
   int*  pos;   //index into `heap` for each value
   int*  heap;  //max/median/min heap holding indexes into `data`.
   int   N;     //allocated size.
   int   idx;   //position in circular queue
   int   ct;    //count of items in queue
} Mediator;

/*--- Helper Functions ---*/

#define minCt(m) (((m)-&gt;ct-1)/2) //count of items in minheap
#define maxCt(m) (((m)-&gt;ct)/2)   //count of items in maxheap 

//returns 1 if heap[i] &lt; heap[j]
int mmless(Mediator* m, int i, int j)
{
   return ItemLess(m-&gt;data[m-&gt;heap[i]],m-&gt;data[m-&gt;heap[j]]);
}

//swaps items i&amp;j in heap, maintains indexes
int mmexchange(Mediator* m, int i, int j)
{
   int t = m-&gt;heap[i];
   m-&gt;heap[i]=m-&gt;heap[j];
   m-&gt;heap[j]=t;
   m-&gt;pos[m-&gt;heap[i]]=i;
   m-&gt;pos[m-&gt;heap[j]]=j;
   return 1;
}

//swaps items i&amp;j if i&lt;j;  returns true if swapped
int mmCmpExch(Mediator* m, int i, int j)
{
   return (mmless(m,i,j) &amp;&amp; mmexchange(m,i,j));
}

//maintains minheap property for all items below i/2.
void minSortDown(Mediator* m, int i)
{
   for (; i &lt;= minCt(m); i*=2)
   {  if (i&gt;1 &amp;&amp; i &lt; minCt(m) &amp;&amp; mmless(m, i+1, i)) { ++i; }
      if (!mmCmpExch(m,i,i/2)) { break; }
   }
}

//maintains maxheap property for all items below i/2. (negative indexes)
void maxSortDown(Mediator* m, int i)
{
   for (; i &gt;= -maxCt(m); i*=2)
   {  if (i&lt;-1 &amp;&amp; i &gt; -maxCt(m) &amp;&amp; mmless(m, i, i-1)) { --i; }
      if (!mmCmpExch(m,i/2,i)) { break; }
   }
}

//maintains minheap property for all items above i, including median
//returns true if median changed
int minSortUp(Mediator* m, int i)
{
   while (i&gt;0 &amp;&amp; mmCmpExch(m,i,i/2)) i/=2;
   return (i==0);
}

//maintains maxheap property for all items above i, including median
//returns true if median changed
int maxSortUp(Mediator* m, int i)
{
   while (i&lt;0 &amp;&amp; mmCmpExch(m,i/2,i))  i/=2;
   return (i==0);
}

/*--- Public Interface ---*/


//creates new Mediator: to calculate `nItems` running median. 
//mallocs single block of memory, caller must free.
Mediator* MediatorNew(int nItems)
{
   int size = sizeof(Mediator)+nItems*(sizeof(Item)+sizeof(int)*2);
   Mediator* m=  malloc(size);
   m-&gt;data= (Item*)(m+1);
   m-&gt;pos = (int*) (m-&gt;data+nItems);
   m-&gt;heap = m-&gt;pos+nItems + (nItems/2); //points to middle of storage.
   m-&gt;N=nItems;
   m-&gt;ct = m-&gt;idx = 0;
   while (nItems--)  //set up initial heap fill pattern: median,max,min,max,...
   {  m-&gt;pos[nItems]= ((nItems+1)/2) * ((nItems&amp;1)?-1:1);
      m-&gt;heap[m-&gt;pos[nItems]]=nItems;
   }
   return m;
}


//Inserts item, maintains median in O(lg nItems)
void MediatorInsert(Mediator* m, Item v)
{
   int isNew=(m-&gt;ct&lt;m-&gt;N);
   int p = m-&gt;pos[m-&gt;idx];
   Item old = m-&gt;data[m-&gt;idx];
   m-&gt;data[m-&gt;idx]=v;
   m-&gt;idx = (m-&gt;idx+1) % m-&gt;N;
   m-&gt;ct+=isNew;
   if (p&gt;0)         //new item is in minHeap
   {  if (!isNew &amp;&amp; ItemLess(old,v)) { minSortDown(m,p*2);  }
      else if (minSortUp(m,p)) { maxSortDown(m,-1); }
   }
   else if (p&lt;0)   //new item is in maxheap
   {  if (!isNew &amp;&amp; ItemLess(v,old)) { maxSortDown(m,p*2); }
      else if (maxSortUp(m,p)) { minSortDown(m, 1); }
   }
   else            //new item is at median
   {  if (maxCt(m)) { maxSortDown(m,-1); }
      if (minCt(m)) { minSortDown(m, 1); }
   }
}

//returns median item (or average of 2 when item count is even)
Item MediatorMedian(Mediator* m)
{
   Item v= m-&gt;data[m-&gt;heap[0]];
   if ((m-&gt;ct&amp;1)==0) { v= ItemMean(v,m-&gt;data[m-&gt;heap[-1]]); }
   return v;
}


/*--- Test Code ---*/
#include &lt;stdio.h&gt;
void PrintMaxHeap(Mediator* m)
{
   int i;
   if(maxCt(m))
      printf("Max: %3d",m-&gt;data[m-&gt;heap[-1]]);
   for (i=2;i&lt;=maxCt(m);++i)
   {
      printf("|%3d ",m-&gt;data[m-&gt;heap[-i]]);
      if(++i&lt;=maxCt(m)) printf("%3d",m-&gt;data[m-&gt;heap[-i]]);
   }
   printf("\n");
}
void PrintMinHeap(Mediator* m)
{
   int i;
   if(minCt(m))
      printf("Min: %3d",m-&gt;data[m-&gt;heap[1]]);
   for (i=2;i&lt;=minCt(m);++i)
   {
      printf("|%3d ",m-&gt;data[m-&gt;heap[i]]);
      if(++i&lt;=minCt(m)) printf("%3d",m-&gt;data[m-&gt;heap[i]]);
   }
   printf("\n");
}

void ShowTree(Mediator* m)
{
   PrintMaxHeap(m);
   printf("Mid: %3d\n",m-&gt;data[m-&gt;heap[0]]);
   PrintMinHeap(m);
   printf("\n");
}

int main(int argc, char* argv[])
{
   int i,v;
   Mediator* m = MediatorNew(15);

   for (i=0;i&lt;30;i++)
   {
      v = rand()&amp;127;
      printf("Inserting %3d \n",v);
      MediatorInsert(m,v);
      v=MediatorMedian(m);
      printf("Median = %3d.\n\n",v);
      ShowTree(m);
   }
}