diff --git a/src/psmoveservice/Device/View/ServerControllerView.cpp b/src/psmoveservice/Device/View/ServerControllerView.cpp
index 5a0ec633..2a88a6d8 100644
--- a/src/psmoveservice/Device/View/ServerControllerView.cpp
+++ b/src/psmoveservice/Device/View/ServerControllerView.cpp
@@ -1441,8 +1441,15 @@ init_filters_for_psmove(
 
         position_filter->setFilterSpace(filterSpace);
 
-        // Use the LowPass + exponential filter by default
-        position_filter->setFusionType(PositionFilter::FusionTypeLowPassExponential);
+        //###HipsterSloth $TODO Proper filter selection
+		if (psmove_config->enable_filtered_velocity)
+		{
+			position_filter->setFusionType(PositionFilter::FusionTypeLowPassExponential);
+		}
+		else
+		{
+			position_filter->setFusionType(PositionFilter::FusionTypeLowPassOptical);
+		}
         position_filter->setAccelerometerNoiseRadius(psmove_config->accelerometer_noise_radius);
         position_filter->setMaxVelocity(psmove_config->max_velocity);
     }
diff --git a/src/psmoveservice/Filter/PositionFilter.cpp b/src/psmoveservice/Filter/PositionFilter.cpp
index ebe294ba..8180bd9e 100644
--- a/src/psmoveservice/Filter/PositionFilter.cpp
+++ b/src/psmoveservice/Filter/PositionFilter.cpp
@@ -2,13 +2,8 @@
 #include "PositionFilter.h"
 #include "MathEigen.h"
 #include "ServerLog.h"
-#include <deque>
-#include <list>
 
 //-- constants -----
-// Max length of the position history we keep
-#define k_position_history_max 16
-
 // The max distance between samples that we apply low pass filter on the optical position filter
 #define k_max_lowpass_smoothing_distance 10.f * k_centimeters_to_meters // meters
 
@@ -56,8 +51,6 @@ struct PositionSensorFusionState
     Eigen::Vector3f origin_position; // meters
 
 	// required for smoothing position to get velocity
-	std::list<float> exp_delta_time_list;
-	std::list<Eigen::Vector3f> exp_position_list;
 	float exp_delta_time;
 	Eigen::Vector3f exp_velocity;
 
diff --git a/src/psmoveservice/PSMoveController/PSMoveController.cpp b/src/psmoveservice/PSMoveController/PSMoveController.cpp
index 03111b49..75c95a02 100644
--- a/src/psmoveservice/PSMoveController/PSMoveController.cpp
+++ b/src/psmoveservice/PSMoveController/PSMoveController.cpp
@@ -249,6 +249,8 @@ PSMoveControllerConfig::config2ptree()
 
 	writeTrackingColor(pt, tracking_color_id);
 
+	pt.put("enable_filtered_velocity", enable_filtered_velocity);
+
     return pt;
 }
 
@@ -315,6 +317,8 @@ PSMoveControllerConfig::ptree2config(const boost::property_tree::ptree &pt)
         max_velocity= pt.get<float>("PositionFilter.MaxVelocity", max_velocity);
 
 		tracking_color_id= static_cast<eCommonTrackingColorID>(readTrackingColor(pt));
+
+		enable_filtered_velocity= pt.get<bool>("enable_filtered_velocity", enable_filtered_velocity);
     }
     else
     {
diff --git a/src/psmoveservice/PSMoveController/PSMoveController.h b/src/psmoveservice/PSMoveController/PSMoveController.h
index 6a63063b..745022c2 100644
--- a/src/psmoveservice/PSMoveController/PSMoveController.h
+++ b/src/psmoveservice/PSMoveController/PSMoveController.h
@@ -44,6 +44,7 @@ class PSMoveControllerConfig : public PSMoveConfig
         , max_position_quality_screen_area(k_real_pi*20.f*20.f) // lightbulb at ideal range is about 40px by 40px 
         , max_velocity(1.f)
 		, tracking_color_id(eCommonTrackingColorID::INVALID_COLOR)
+		, enable_filtered_velocity(true)
     {
         magnetometer_identity.clear();
         magnetometer_center.clear();
@@ -91,6 +92,9 @@ class PSMoveControllerConfig : public PSMoveConfig
 
 	// The assigned tracking color for this controller
 	eCommonTrackingColorID tracking_color_id;
+
+	// Chicken switch for velocity filtering
+	bool enable_filtered_velocity;
 };
 
 // https://code.google.com/p/moveonpc/wiki/InputReport