Wasn't setting filter on velocity.

quad/src/quad_app/PID.h

@@ -39,7 +39,7 @@
 #define YPOS_KP 0.55
 #define YPOS_KI 0.0075
 #define YPOS_KD 0.0
-#define YPOS_ALPHA 0.88
+#define YPOS_ALPHA 0
 
 
 //Pitch constants
@@ -58,7 +58,7 @@
 #define XPOS_KP 0.55
 #define XPOS_KI 0.0075
 #define XPOS_KD 0.0
-#define XPOS_ALPHA 0.88
+#define XPOS_ALPHA 0
 
 
 //Throttle constants

quad/src/quad_app/control_algorithm.c

@@ -214,10 +214,11 @@ int control_algorithm_init(parameter_t * ps)
     // Set velocity constants
     graph_set_param_val(graph, ps->x_vel_pid, PID_KP, XVEL_KP);
     graph_set_param_val(graph, ps->x_vel_pid, PID_KD, XVEL_KD);
-    graph_set_param_val(graph, ps->x_vel_pid, PID_ALPHA, XVEL_ALPHA);
     graph_set_param_val(graph, ps->y_vel_pid, PID_KP, YVEL_KP);
     graph_set_param_val(graph, ps->y_vel_pid, PID_KD, YVEL_KD);
-    graph_set_param_val(graph, ps->y_vel_pid, PID_ALPHA, YVEL_ALPHA);
+    // Differentiators
+    graph_set_param_val(graph, ps->x_vel, PID_ALPHA, XVEL_ALPHA);
+    graph_set_param_val(graph, ps->y_vel, PID_ALPHA, YVEL_ALPHA);
 
     // Set X/Y/Alt constants
     graph_set_param_val(graph, ps->x_pos_pid, PID_KP, XPOS_KP);