diff --git a/quad/src/gen_diagram/network.dot b/quad/src/gen_diagram/network.dot
index 7321d05e93f9aec20e120f62657e66ad43cab924..25a5dc165c2dac141e2c6694b707ec96db8eaf4a 100644
--- a/quad/src/gen_diagram/network.dot
+++ b/quad/src/gen_diagram/network.dot
@@ -3,12 +3,12 @@ rankdir="LR"
 "Roll PID"[shape=record
 label="<f0>Roll PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200] |<f7> [alpha=0.000]"]
 "Roll" -> "Roll PID":f1 [label="Constant"]
-"RC Roll" -> "Roll PID":f2 [label="Constant"]
+"Y Vel PID" -> "Roll PID":f2 [label="Correction"]
 "Ts_IMU" -> "Roll PID":f3 [label="Constant"]
 "Pitch PID"[shape=record
 label="<f0>Pitch PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200] |<f7> [alpha=0.000]"]
 "Pitch trim add" -> "Pitch PID":f1 [label="Sum"]
-"RC Pitch" -> "Pitch PID":f2 [label="Constant"]
+"X Vel PID" -> "Pitch PID":f2 [label="Correction"]
 "Ts_IMU" -> "Pitch PID":f3 [label="Constant"]
 "Yaw PID"[shape=record
 label="<f0>Yaw PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=2.600] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
@@ -28,7 +28,7 @@ label="<f0>Pitch Rate PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |
 "Yaw Rate PID"[shape=record
 label="<f0>Yaw Rate PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=29700.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
 "dPsi" -> "Yaw Rate PID":f1 [label="Constant"]
-"RC Yaw" -> "Yaw Rate PID":f2 [label="Constant"]
+"Yaw PID" -> "Yaw Rate PID":f2 [label="Correction"]
 "Ts_IMU" -> "Yaw Rate PID":f3 [label="Constant"]
 "X pos PID"[shape=record
 label="<f0>X pos PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.015] |<f5> [Ki=-0.005] |<f6> [Kd=-0.030] |<f7> [alpha=0.000]"]
@@ -106,7 +106,7 @@ label="<f0>RC Yaw  |<f1> [Constant=0.000]"]
 label="<f0>RC Throttle  |<f1> [Constant=0.000]"]
 "Signal Mixer"[shape=record
 label="<f0>Signal Mixer  |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4> --\>Yaw"]
-"RC Throttle" -> "Signal Mixer":f1 [label="Constant"]
+"T trim add" -> "Signal Mixer":f1 [label="Sum"]
 "P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
 "R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
 "Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]
@@ -114,4 +114,36 @@ label="<f0>Signal Mixer  |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4>
 label="<f0>Ts_IMU  |<f1> [Constant=0.005]"]
 "Ts_VRPN"[shape=record
 label="<f0>Ts_VRPN  |<f1> [Constant=0.040]"]
+"zero"[shape=record
+label="<f0>zero  |<f1> [Constant=0.000]"]
+"X Vel PID"[shape=record
+label="<f0>X Vel PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
+"X Vel" -> "X Vel PID":f1 [label="Correction"]
+"X Vel Clamp" -> "X Vel PID":f2 [label="Bounded"]
+"Ts_VRPN" -> "X Vel PID":f3 [label="Constant"]
+"Y Vel PID"[shape=record
+label="<f0>Y Vel PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
+"Y Vel" -> "Y Vel PID":f1 [label="Correction"]
+"Y vel Clamp" -> "Y Vel PID":f2 [label="Bounded"]
+"Ts_VRPN" -> "Y Vel PID":f3 [label="Constant"]
+"X Vel"[shape=record
+label="<f0>X Vel  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.000]"]
+"VRPN X" -> "X Vel":f1 [label="Constant"]
+"zero" -> "X Vel":f2 [label="Constant"]
+"Ts_VRPN" -> "X Vel":f3 [label="Constant"]
+"Y Vel"[shape=record
+label="<f0>Y Vel  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.000]"]
+"VRPN Y" -> "Y Vel":f1 [label="Constant"]
+"zero" -> "Y Vel":f2 [label="Constant"]
+"Ts_VRPN" -> "Y Vel":f3 [label="Constant"]
+"X Vel Clamp"[shape=record
+label="<f0>X Vel Clamp  |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]"]
+"X pos PID" -> "X Vel Clamp":f1 [label="Correction"]
+"Y vel Clamp"[shape=record
+label="<f0>Y vel Clamp  |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]"]
+"Y pos PID" -> "Y vel Clamp":f1 [label="Correction"]
+"X Stick Gain"[shape=record
+label="<f0>X Stick Gain  |<f1> --\>Input |<f2> [Gain=5.000]"]
+"Y Stick Gain"[shape=record
+label="<f0>Y Stick Gain  |<f1> --\>Input |<f2> [Gain=-5.000]"]
 }
\ No newline at end of file
diff --git a/quad/src/gen_diagram/network.png b/quad/src/gen_diagram/network.png
index 17aa66c2d549bb483b5812ec7e80e0d53933d358..6e1c3ebc4db038a6d42839315233e453f2e123fe 100644
Binary files a/quad/src/gen_diagram/network.png and b/quad/src/gen_diagram/network.png differ
diff --git a/quad/src/graph_blocks/node_pid.c b/quad/src/graph_blocks/node_pid.c
index eb166b3c9338852c5ca32421f601d2fa5a112195..e776b7ed28e9295c9f30be9adb104ae3e9598647 100644
--- a/quad/src/graph_blocks/node_pid.c
+++ b/quad/src/graph_blocks/node_pid.c
@@ -73,7 +73,7 @@ static void pid_computation(void *state, const double* params, const double *inp
     pid_state->last_filtered = D; // Store filtered value for next filter iteration
     pid_state->prev_val = inputs[PID_CUR_POINT]; // Store the current error into the state
 
-    outputs[PID_CORRECTION] = P + I + D; // Store the computed correction
+    outputs[PID_CORRECTION] = P + I - D; // Store the computed correction
 }
 
 // This function sets the accumulated error and previous error to 0
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 0391ca2dd23cca86ae5b5c0fd9191da3e28eaca1..710f26b34144c2827d46aa0363575541a12aae97 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -19,9 +19,11 @@
 
 void connect_autonomous(parameter_t* ps) {
 	struct computation_graph* graph = ps->graph;
-	graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->x_pos_pid, PID_CORRECTION);
-	graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->y_pos_pid, PID_CORRECTION);
-	//graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
+	//graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->x_pos_pid, PID_CORRECTION);
+	//graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->y_pos_pid, PID_CORRECTION);
+	graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->x_vel_pid, PID_CORRECTION);
+	graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->y_vel_pid, PID_CORRECTION);
+	graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
 	graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_pid, PID_CORRECTION);
 }
 
@@ -195,6 +197,58 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MIN, -PWM_DIFF_BOUNDS);
     graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MAX, PWM_DIFF_BOUNDS);
 
+
+    // Velocity stuff
+    int zero_block = graph_add_defined_block(graph, BLOCK_CONSTANT, "zero");
+
+    ps->x_vel_pid = graph_add_defined_block(graph, BLOCK_PID, "X Vel PID");
+    ps->y_vel_pid = graph_add_defined_block(graph, BLOCK_PID, "Y Vel PID");
+    ps->x_vel = graph_add_defined_block(graph, BLOCK_PID, "X Vel");
+    ps->y_vel = graph_add_defined_block(graph, BLOCK_PID, "Y Vel");
+    ps->x_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "X Vel Clamp");
+    ps->y_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "Y vel Clamp");
+    ps->vel_x_gain = graph_add_defined_block(graph, BLOCK_GAIN, "X Stick Gain");
+    ps->vel_y_gain = graph_add_defined_block(graph, BLOCK_GAIN, "Y Stick Gain");
+
+    graph_set_source(graph, ps->x_vel_pid, PID_DT, ps->pos_time, CONST_VAL);
+    graph_set_source(graph, ps->y_vel_pid, PID_DT, ps->pos_time, CONST_VAL);
+    graph_set_source(graph, ps->x_vel, PID_DT, ps->pos_time, CONST_VAL);
+    graph_set_source(graph, ps->y_vel, PID_DT, ps->pos_time, CONST_VAL);
+
+    graph_set_source(graph, ps->x_vel_pid, PID_SETPOINT, ps->x_vel, PID_CORRECTION);
+    graph_set_source(graph, ps->y_vel_pid, PID_SETPOINT, ps->y_vel, PID_CORRECTION);
+    graph_set_source(graph, ps->x_vel_pid, PID_CUR_POINT, ps->x_vel, PID_CORRECTION);
+    graph_set_source(graph, ps->y_vel_pid, PID_CUR_POINT, ps->y_vel, PID_CORRECTION);
+
+    graph_set_source(graph, ps->x_vel, PID_CUR_POINT, ps->vrpn_x, CONST_VAL);
+    graph_set_source(graph, ps->y_vel, PID_CUR_POINT, ps->vrpn_y, CONST_VAL);
+    graph_set_source(graph, ps->x_vel, PID_SETPOINT, zero_block, CONST_VAL);
+    graph_set_source(graph, ps->y_vel, PID_SETPOINT, zero_block, CONST_VAL);
+
+    graph_set_source(graph, ps->x_vel_pid, PID_SETPOINT, ps->x_vel_clamp, GAIN_RESULT);
+    graph_set_source(graph, ps->y_vel_pid, PID_SETPOINT, ps->y_vel_clamp, GAIN_RESULT);
+
+    graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
+    graph_set_source(graph, ps->y_vel_clamp, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);
+    //graph_set_source(graph, ps->vel_x_gain, GAIN_INPUT, ps->rc_pitch, CONST_VAL);
+    //graph_set_source(graph, ps->vel_y_gain, GAIN_INPUT, ps->rc_roll, CONST_VAL);
+    //graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->vel_x_gain, GAIN_RESULT);
+    //graph_set_source(graph, ps->y_vel_clamp, BOUNDS_IN, ps->vel_y_gain, GAIN_RESULT);
+
+    float vel_clamps = 2;
+    graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MIN, -vel_clamps);
+    graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MAX, vel_clamps);
+    graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MIN, -vel_clamps);
+    graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MAX, vel_clamps);
+
+
+    graph_set_param_val(graph, ps->x_vel, PID_KD, 1);
+    graph_set_param_val(graph, ps->y_vel, PID_KD, 1);
+
+    graph_set_param_val(graph, ps->vel_x_gain, GAIN_GAIN, 5);
+    graph_set_param_val(graph, ps->vel_y_gain, GAIN_GAIN, -5);
+
+
     // Set trims
     graph_set_param_val(graph, ps->pitch_trim, CONST_SET, 0.02);
 
diff --git a/quad/src/quad_app/iic_utils.c b/quad/src/quad_app/iic_utils.c
index 2f3875a0f035a9c1f2f98f4251d07fed6e4fc329..1776bd8e50ca781bac751bbf0c94c78c3889c8f8 100644
--- a/quad/src/quad_app/iic_utils.c
+++ b/quad/src/quad_app/iic_utils.c
@@ -30,7 +30,7 @@ int iic0_mpu9150_start(){
 	// Set clock reference to Z Gyro
 	iic0_mpu9150_write(0x6B, 0x03);
 	// Configure Digital Low/High Pass filter
-	iic0_mpu9150_write(0x1A,0x05); // Level 5 low pass on gyroscope
+	iic0_mpu9150_write(0x1A,0x03); // Level 5 low pass on gyroscope
 
 	// Configure Gyro to 2000dps, Accel. to +/-8G
 	iic0_mpu9150_write(0x1B, 0x18);
diff --git a/quad/src/quad_app/initialize_components.c b/quad/src/quad_app/initialize_components.c
index 57d3e2b96eaef8951425652c4fb61ac5ec4b1529..967eaa6294b342641fc1dcdfb0b3327ea4490427 100644
--- a/quad/src/quad_app/initialize_components.c
+++ b/quad/src/quad_app/initialize_components.c
@@ -99,5 +99,7 @@ int init_structs(modular_structs_t *structs) {
   if(sensor_init(&structs->raw_sensor_struct, &structs->sensor_struct) == -1)
     return -1;
 
+  sensor_processing_init(&structs->sensor_struct);
+
   return 0;
 }
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 3e67c14aa36862cae1ad885955e944a86056007b..e67a075394ee6099d89f0c326a07f656a72694ee 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -95,6 +95,7 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
 	char* rad_s = "rad/s";
 	char* pwm_val = "10ns_dutycycle";
 	char* m = "m";
+	char* m_s = "m/s";
 	addOutputToLog(log_struct, ps->alt_pid, PID_CORRECTION, pwm_val);
 	addOutputToLog(log_struct, ps->x_pos_pid, PID_CORRECTION, rad);
 	addOutputToLog(log_struct, ps->y_pos_pid, PID_CORRECTION, rad);
@@ -104,7 +105,7 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
 	addOutputToLog(log_struct, ps->pitch_r_pid, PID_CORRECTION, pwm_val);
 	addOutputToLog(log_struct, ps->roll_r_pid, PID_CORRECTION, pwm_val);
 	addOutputToLog(log_struct, ps->yaw_r_pid, PID_CORRECTION, pwm_val);
-	addOutputToLog(log_struct, ps->cur_pitch, CONST_VAL, rad);
+	addOutputToLog(log_struct, ps->pitch_trim_add, CONST_VAL, rad);
 	addOutputToLog(log_struct, ps->cur_roll, CONST_VAL, rad);
 	addOutputToLog(log_struct, ps->cur_yaw, CONST_VAL, rad);
 	addOutputToLog(log_struct, ps->vrpn_x, CONST_VAL, m);
@@ -122,6 +123,10 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
 	addOutputToLog(log_struct, ps->rc_throttle, PID_CORRECTION, pwm_val);
 	addOutputToLog(log_struct, ps->rc_pitch, PID_CORRECTION, pwm_val);
 	addOutputToLog(log_struct, ps->rc_roll, PID_CORRECTION, pwm_val);
+	addOutputToLog(log_struct, ps->x_vel_pid, PID_CORRECTION, rad);
+	addOutputToLog(log_struct, ps->y_vel_pid, PID_CORRECTION, rad);
+	addOutputToLog(log_struct, ps->x_vel, PID_CORRECTION, m_s);
+	addOutputToLog(log_struct, ps->y_vel, PID_CORRECTION, m_s);
 
 	// TODO: Make this not stupid. Adding 6 for IMU and 1 for timestamp
 	row_size = n_outputs + n_params + 6 + 1;
@@ -184,12 +189,27 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
 		send_data(&hardware_struct->uart, LOG_ID, 0, buf.str, buf.size);
 		return;
 	}
+
+	int i;
 	// Comment header
 	safe_sprintf_cat(&buf, "# MicroCART On-board Quad Log\n# Sample size: %d\n", arrayIndex);
+
+	// List Pid Constants
+	for (i = 0; i < ps->graph->n_nodes; ++i) {
+		struct graph_node* node = &ps->graph->nodes[i];
+		if (node->type->type_id == BLOCK_PID) {
+			double kp, ki, kd;
+			kp = graph_get_param_val(ps->graph, i, 0);
+			ki = graph_get_param_val(ps->graph, i, 1);
+			kd = graph_get_param_val(ps->graph, i, 2);
+			safe_sprintf_cat(&buf, "# %s :\tKp = %lf Ki = %lf Kd = %lf\n", node->name, kp, ki, kd);
+
+		}
+	}
+
 	// Header names for the pre-defined values
 	safe_sprintf_cat(&buf, "%%Time\taccel_x\taccel_y\taccel_z\tgyro_x\tgyro_y\tgyro_z");
 
-	int i;
 	// Print all the recorded block parameters
 	for (i = 0; i < n_params; i++) {
 		const char* block_name = ps->graph->nodes[log_params[i].block_id].name;
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index dee473219dad3fff0beacfd192de2a99526a6a1e..3e3b48bd1bbcb16dca49f198494ce05f847b6dfd 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -14,7 +14,7 @@
 #include "timer.h"
 #include <math.h>
 
-#define ALPHA 0.98
+#define ALPHA 0.99
 
 int sensor_processing_init(sensor_t* sensor_struct) {
 	float a0 = 0.0200833310260;
@@ -56,13 +56,14 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 	// |local y|  = |sin(yaw angle)   cos(yaw angle)  0| |camera given y|
 	// |local z|	|       0               0         1| |camera given z|
 
+
 	sensor_struct->currentQuadPosition.x_pos =
-			sensor_struct->currentQuadPosition.x_pos * cos(sensor_struct->currentQuadPosition.yaw) +
-			sensor_struct->currentQuadPosition.y_pos * -sin(sensor_struct->currentQuadPosition.yaw);
+			raw_sensor_struct->currentQuadPosition.x_pos * cos(raw_sensor_struct->currentQuadPosition.yaw) +
+			raw_sensor_struct->currentQuadPosition.y_pos * -sin(raw_sensor_struct->currentQuadPosition.yaw);
 
 	sensor_struct->currentQuadPosition.y_pos =
-			sensor_struct->currentQuadPosition.x_pos * sin(sensor_struct->currentQuadPosition.yaw) +
-			sensor_struct->currentQuadPosition.y_pos * cos(sensor_struct->currentQuadPosition.yaw);
+			raw_sensor_struct->currentQuadPosition.x_pos * sin(raw_sensor_struct->currentQuadPosition.yaw) +
+			raw_sensor_struct->currentQuadPosition.y_pos * cos(raw_sensor_struct->currentQuadPosition.yaw);
 
 
 	// Calculate Euler angles and velocities using Gimbal Equations below
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index a268af2ca6e796a3ddb171c74d7d0a3ca6130d30..d016134432b8e058601904a5013a296b7ecd7b00 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -348,6 +348,15 @@ typedef struct parameter_t {
 	int throttle_trim_add;
 	int pitch_trim;
 	int pitch_trim_add;
+	// Velocity nodes
+	int x_vel_pid;
+	int y_vel_pid;
+	int x_vel;
+	int y_vel;
+	int x_vel_clamp;
+	int y_vel_clamp;
+	int vel_x_gain;
+	int vel_y_gain;
 } parameter_t;
 
 /**