diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
index c9dcb2b173..10ceffd333 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
@@ -28,12 +28,17 @@
 
 // AVR LibC Includes
 #include "WConstants.h"
-
 #include "AP_RangeFinder_MaxsonarXL.h"
 
 // Public Methods //////////////////////////////////////////////////////////////
 void AP_RangeFinder_MaxsonarXL::init(int analogPort)
 {
+    // local variables
+    int i;
+	
+	// set the given analog port to an input
+	pinMode(analogPort, INPUT);
+	
     // initialise everything
     _analogPort = analogPort;
 	max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE;
@@ -41,6 +46,10 @@ void AP_RangeFinder_MaxsonarXL::init(int analogPort)
 	
 	// make first call to read to get initial distance
 	read();
+	
+	// initialise history
+	for( i=0; i<AP_RANGEFINDER_NUM_AVERAGES; i++ )
+	    _history[i] = distance;	
 }
 
 // Read Sensor data
@@ -52,17 +61,6 @@ int AP_RangeFinder_MaxsonarXL::read()
 	// for this sensor, the sensor value is the distance in cm! nice and easy!
 	distance = constrain(raw_value,min_distance,max_distance);
 	
-	// implement filter
-    //switch( _filterType ) {
-	//    case AP_RANGEFINDER_FILTER_LIMITED_CHANGE:
-	//	    distance = constrain(
-	//	    break;
-	//	case AP_RANGEFINDER_FILTER_NONE:
-	//    default:
-	//	    distance = tempDistance;
-	//	    break;
-	//}
-	
 	// return distance
-	return distance;
+	return filter(distance);
 }
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp
index 1072536d17..975883cdec 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp
@@ -28,12 +28,17 @@
 
 // AVR LibC Includes
 #include "WConstants.h"
-
 #include "AP_RangeFinder_SharpGP2Y.h"
 
 // Public Methods //////////////////////////////////////////////////////////////
 void AP_RangeFinder_SharpGP2Y::init(int analogPort)
 {
+    // local variables
+    int i;
+	
+	// set the given analog port to an input
+	pinMode(analogPort, INPUT);
+	
     // initialise everything
     _analogPort = analogPort;
 	max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE;
@@ -41,6 +46,10 @@ void AP_RangeFinder_SharpGP2Y::init(int analogPort)
 	
 	// make first call to read to get initial distance
 	read();
+	
+	// initialise history
+	for( i=0; i<AP_RANGEFINDER_NUM_AVERAGES; i++ )
+	    _history[i] = distance;
 }
 
 // Read Sensor data
@@ -48,19 +57,11 @@ int AP_RangeFinder_SharpGP2Y::read()
 {
     // read raw sensor value and convert to distance
     raw_value = analogRead(_analogPort);
-	distance = constrain(14500/raw_value,min_distance,max_distance); 
-	
-	// implement filter
-    //switch( _filterType ) {
-	//    case AP_RANGEFINDER_FILTER_LIMITED_CHANGE:
-	//	    distance = constrain(
-	//	    break;
-	//	case AP_RANGEFINDER_FILTER_NONE:
-	//    default:
-	//	    distance = tempDistance;
-	//	    break;
-	//}
+	if( raw_value == 0 )
+	    distance = max_distance;
+	else
+	    distance = constrain(14500/raw_value,min_distance,max_distance); 
 	
 	// return distance
-	return distance;
+	return filter(distance);
 }
diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp
new file mode 100644
index 0000000000..0a07284e07
--- /dev/null
+++ b/libraries/AP_RangeFinder/RangeFinder.cpp
@@ -0,0 +1,37 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
+/*
+	AP_RangeFinder.cpp - Arduino Library for Sharpe GP2Y0A02YK0F 
+	infrared proximity sensor
+	Code by Jose Julio and Randy Mackay. DIYDrones.com
+
+	This library is free software; you can redistribute it and/or
+    modify it under the terms of the GNU Lesser General Public
+    License as published by the Free Software Foundation; either
+    version 2.1 of the License, or (at your option) any later version.
+
+	This has the basic functions that all RangeFinders need implemented
+*/
+
+// AVR LibC Includes
+#include "WConstants.h"
+#include "RangeFinder.h"
+
+// Public Methods //////////////////////////////////////////////////////////////
+void RangeFinder::set_orientation(int x, int y, int z)
+{
+    orientation_x = x; 
+	orientation_y = y; 
+	orientation_z = z;
+}
+
+// Protected Methods //////////////////////////////////////////////////////////
+int RangeFinder::filter(int latestValue)
+{
+    int i;
+    int total = 0;
+    _history_ptr = (_history_ptr + 1) % AP_RANGEFINDER_NUM_AVERAGES;
+    _history[_history_ptr] = latestValue;
+	for(i=0; i<AP_RANGEFINDER_NUM_AVERAGES; i++ )
+	    total += _history[i];
+    return total / AP_RANGEFINDER_NUM_AVERAGES;
+}
diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h
index 3012aba640..0db580c045 100644
--- a/libraries/AP_RangeFinder/RangeFinder.h
+++ b/libraries/AP_RangeFinder/RangeFinder.h
@@ -3,22 +3,36 @@
 
 #include <inttypes.h>
 
-//#define AP_RANGEFINDER_FILTER_NONE 0
-//#define AP_RANGEFINDER_FILTER_LIMITED_CHANGE 1
+#define AP_RANGEFINDER_ORIENTATION_FRONT		  0, 10,  0
+#define AP_RANGEFINDER_ORIENTATION_RIGHT		-10,  0,  0
+#define AP_RANGEFINDER_ORIENTATION_BACK			  0,-10,  0
+#define AP_RANGEFINDER_ORIENTATION_LEFT			 10,  0,  0
+#define AP_RANGEFINDER_ORIENTATION_UP			  0,  0,-10
+#define AP_RANGEFINDER_ORIENTATION_DOWN			  0,  0, 10
+#define AP_RANGEFINDER_ORIENTATION_FRONT_RIGHT 	 -5, -5,  0
+#define AP_RANGEFINDER_ORIENTATION_BACK_RIGHT 	 -5, -5,  0
+#define AP_RANGEFINDER_ORIENTATION_BACK_LEFT 	  5, -5,  0
+#define AP_RANGEFINDER_ORIENTATION_FRONT_LEFT 	  5,  5,  0
+
+#define AP_RANGEFINDER_NUM_AVERAGES 4
 
 class RangeFinder
 {
-  protected:
+  public:
     int _analogPort;  // the port to which the sensor is connected
-	int _filterType;
+	int _history_ptr;
+	int _history[AP_RANGEFINDER_NUM_AVERAGES];
   public:
 	int raw_value;  // raw value from the sensor
 	int distance; // distance in cm
 	int max_distance;  // maximum measurable distance (in cm)
 	int min_distance;  // minimum measurable distance (in cm)
+	int orientation_x, orientation_y, orientation_z;
+
+	int filter(int latestValue);  // returns the average of the last AP_RANGEFINDER_NUM_AVERAGES values
 	
 	virtual void init(int analogPort);
-	//virtual void set_filter(int filterType) { _filterType = filterType; };
+    virtual void set_orientation(int x, int y, int z);
 	virtual int read();   // read value from analog port and return distance in cm
 };
 #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 d156b4698a..521bf23a1d 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
@@ -3,13 +3,13 @@
   Code by DIYDrones.com
 */
 
-#include <AP_RangeFinder.h>        // Sonar library
+#include <AP_RangeFinder.h>        // Range finder library
 
-#define RF_PIN 5 // the far back-right pin on the oilpan (near the CLI switch)
+#define RF_PIN A5 // the far back-right pin on the oilpan (near the CLI switch)
 
 // create the range finder object
-//AP_RangeFinder_SharpGP2Y aRF;
-AP_RangeFinder_MaxsonarXL aRF;
+AP_RangeFinder_SharpGP2Y aRF;
+//AP_RangeFinder_MaxsonarXL aRF;
 
 void setup()
 {
@@ -20,11 +20,12 @@ void setup()
 
 void loop()
 {   
+    int i = 0;
     Serial.print("dist:");
     Serial.print(aRF.read());
     Serial.print("\traw:");
-    Serial.print(aRF.raw_value);    
+    Serial.print(aRF.raw_value); 
     Serial.println();
-    delay(100); 
+    delay(50); 
 }
 
diff --git a/libraries/AP_RangeFinder/keywords.txt b/libraries/AP_RangeFinder/keywords.txt
index e2a4bbaf85..e7055b1e52 100644
--- a/libraries/AP_RangeFinder/keywords.txt
+++ b/libraries/AP_RangeFinder/keywords.txt
@@ -4,7 +4,12 @@ AP_RangeFinder_SharpGP2Y	KEYWORD1
 AP_RangeFinder_MaxsonarXL	KEYWORD1
 init	KEYWORD2
 read	KEYWORD2
+filter	KEYWORD2
+set_orientation	KEYWORD2
 raw_value	KEYWORD2
 distance	KEYWORD2
 max_distance	KEYWORD2
 min_distance	KEYWORD2
+orientation_x	KEYWORD2
+orientation_y	KEYWORD2
+orientation_z	KEYWORD2
\ No newline at end of file