diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
index 20915092d6..e5e865e615 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
@@ -35,7 +35,7 @@
 
 // Constructor //////////////////////////////////////////////////////////////
 
-AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter):
+AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, FilterInt16 *filter):
 		RangeFinder(source, filter),
 		_scaler(AP_RANGEFINDER_MAXSONARXL_SCALER)
 	{
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h
index 12780a6dfc..63b8065e00 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h
@@ -24,7 +24,7 @@
 class AP_RangeFinder_MaxsonarXL : public RangeFinder
 {
   public:
-	AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter);
+	AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, FilterInt16 *filter);
 	int		convert_raw_to_distance(int _raw_value) { return _raw_value * _scaler; }   // read value from analog port and return distance in cm
 	float	calculate_scaler(int sonar_type, float adc_refence_voltage);
 
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp
index 3f08bb7dc7..1e43441159 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp
@@ -35,7 +35,7 @@
 
 // Constructor //////////////////////////////////////////////////////////////
 
-AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, ModeFilter *filter) :
+AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, FilterInt16 *filter) :
 	RangeFinder(source, filter)
 {
     max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE;
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h
index 4f5a328979..b5d92f3f16 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h
@@ -9,7 +9,7 @@
 class AP_RangeFinder_SharpGP2Y : public RangeFinder
 {
   public:
-	AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, ModeFilter *filter);
+	AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, FilterInt16 *filter);
 	int convert_raw_to_distance(int _raw_value) {
 		if( _raw_value == 0 )
 			return max_distance;
diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp
index 63945e5ab7..6b1ae09370 100644
--- a/libraries/AP_RangeFinder/RangeFinder.cpp
+++ b/libraries/AP_RangeFinder/RangeFinder.cpp
@@ -43,7 +43,7 @@ int RangeFinder::read()
 	// ensure distance is within min and max
 	temp_dist = constrain(temp_dist, min_distance, max_distance);
 
-	distance = _mode_filter->get_filtered_with_sample(temp_dist);
+	distance = _mode_filter->apply(temp_dist);
 	return distance;
 }
 
diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h
index 1870cd5a94..03768ba181 100644
--- a/libraries/AP_RangeFinder/RangeFinder.h
+++ b/libraries/AP_RangeFinder/RangeFinder.h
@@ -4,7 +4,7 @@
 #include <stdlib.h>
 #include <inttypes.h>
 #include "../AP_AnalogSource/AP_AnalogSource.h"
-#include "../ModeFilter/ModeFilter.h" // ArduPilot Mega RC Library
+#include "../Filter/Filter.h" // Filter library
 
 /*
 #define AP_RANGEFINDER_ORIENTATION_FRONT		  0, 10,  0
@@ -22,7 +22,7 @@
 class RangeFinder
 {
   protected:
-	RangeFinder(AP_AnalogSource * source, ModeFilter *filter) :
+	RangeFinder(AP_AnalogSource * source, FilterInt16 *filter) :
 		_analog_source(source),
 		_mode_filter(filter) {}
   public:
@@ -38,6 +38,6 @@ class RangeFinder
 	virtual int read();   // read value from sensor and return distance in cm
 
 	AP_AnalogSource *_analog_source;
-	ModeFilter  *_mode_filter;
+	FilterInt16 *_mode_filter;
 };
 #endif
diff --git a/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde b/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde
index b26d1aab44..2d5864baef 100644
--- a/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde
+++ b/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde
@@ -4,6 +4,9 @@
 */
 
 // includes
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>
 #include <AP_RangeFinder.h>     // Range finder library
 #include <Arduino_Mega_ISR_Registry.h>
 #include <AP_PeriodicProcess.h>
@@ -11,6 +14,11 @@
 #include <AP_AnalogSource.h>
 #include <ModeFilter.h>			// mode filter
 
+////////////////////////////////////////////////////////////////////////////////
+// Serial ports
+////////////////////////////////////////////////////////////////////////////////
+FastSerialPort0(Serial);        // FTDI/console
+
 // comment out line below if using APM2 or analog pin instead of APM1's built in ADC
 #define USE_ADC_ADS7844  // use APM1's built in ADC and connect sonar to pitot tube
 
@@ -24,7 +32,7 @@
 
 // declare global instances
 Arduino_Mega_ISR_Registry isr_registry;
-ModeFilter mode_filter;
+ModeFilterInt16 mode_filter(5,2);
 #ifdef USE_ADC_ADS7844
     AP_TimerProcess  adc_scheduler;
     AP_ADC_ADS7844 adc;