diff --git a/quad/src/gen_diagram/network.dot b/quad/src/gen_diagram/network.dot
index 17ba88b4b2b0751fc0cac209f87161a8dbc745e3..8fcbaa05fabb03f710419a8c58bee33255181d55 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=35.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.880]"]
 "Roll" -> "Roll PID":f1 [label="Constant"]
-"Y Vel PID" -> "Roll PID":f2 [label="Correction"]
+"Yaw Correction" -> "Roll PID":f2 [label="Rotated Y"]
 "Ts_IMU" -> "Roll PID":f3 [label="Constant"]
 "Pitch PID"[shape=record
 label="<f0>Pitch PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=35.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.880]"]
 "Pitch trim add" -> "Pitch PID":f1 [label="Sum"]
-"X Vel PID" -> "Pitch PID":f2 [label="Correction"]
+"Yaw Correction" -> "Pitch PID":f2 [label="Rotated X"]
 "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]"]
@@ -67,6 +67,12 @@ label="<f0>Roll  |<f1> [Constant=0.000]"]
 label="<f0>Yaw  |<f1> [Constant=0.000]"]
 "Lidar"[shape=record
 label="<f0>Lidar  |<f1> [Constant=0.000]"]
+"Flow Vel X"[shape=record
+label="<f0>Flow Vel X  |<f1> [Constant=0.000]"]
+"Flow Vel y"[shape=record
+label="<f0>Flow Vel y  |<f1> [Constant=0.000]"]
+"Flow Quality"[shape=record
+label="<f0>Flow Quality  |<f1> [Constant=0.000]"]
 "Pitch trim"[shape=record
 label="<f0>Pitch trim  |<f1> [Constant=0.045]"]
 "Pitch trim add"[shape=record
@@ -132,6 +138,11 @@ label="<f0>X Vel Clamp  |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]
 "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"]
+"Yaw Correction"[shape=record
+label="<f0>Yaw Correction  |<f1> --\>Current Yaw |<f2> --\>X Position |<f3> --\>Y Position"]
+"Yaw" -> "Yaw Correction":f1 [label="Constant"]
+"X Vel PID" -> "Yaw Correction":f2 [label="Correction"]
+"Y Vel PID" -> "Yaw Correction":f3 [label="Correction"]
 "Signal Mixer"[shape=record
 label="<f0>Signal Mixer  |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4> --\>Yaw"]
 "T trim add" -> "Signal Mixer":f1 [label="Sum"]
diff --git a/quad/src/gen_diagram/network.png b/quad/src/gen_diagram/network.png
index 793faa240d892707f7fc09aceb7d6a443f6803d4..f31c3ad387c1823d7fc69f3477e5aa185fbd5292 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/graph_blocks.c b/quad/src/graph_blocks/graph_blocks.c
index 325218bf8d966c0eaca77675fb091ad34e4a28fd..fe92a80106b2ddebe30a6a826f112ce6d2d66e97 100644
--- a/quad/src/graph_blocks/graph_blocks.c
+++ b/quad/src/graph_blocks/graph_blocks.c
@@ -11,7 +11,8 @@ const struct graph_node_type* blockDefs[MAX_BLOCK_TYPES] =
     &node_accum_type,
     &node_bounds_type,
     &node_mixer_type,
-    &node_pid_type
+    &node_pid_type,
+    &node_yaw_rot_type
 };
 
 
diff --git a/quad/src/graph_blocks/graph_blocks.h b/quad/src/graph_blocks/graph_blocks.h
index 812e2a844a15ed0afba19d7fd3f2b78b8a5ea997..d64e1f734239e3d16ff29f7510b6b11b7db5bf59 100644
--- a/quad/src/graph_blocks/graph_blocks.h
+++ b/quad/src/graph_blocks/graph_blocks.h
@@ -10,6 +10,7 @@
 #include "node_bounds.h"
 #include "node_mixer.h"
 #include "node_pid.h"
+#include "node_yaw_rot.h"
 
 /*
  * ---------- How-To ------------
@@ -35,6 +36,7 @@ enum BlockTypes {
     BLOCK_BOUNDS,        // 05
     BLOCK_MIXER,         // 06
     BLOCK_PID,           // 07
+    BLOCK_YAW_ROT,       // 08
     //                        <-- Insert new block type here
     MAX_BLOCK_TYPES
 };
diff --git a/quad/src/graph_blocks/node_accumulator.c b/quad/src/graph_blocks/node_accumulator.c
index 10b96873750e862b9072d0f2738842aa3be69ec7..740d8b7075f2670478a7aae4d9eb94647d2e98df 100644
--- a/quad/src/graph_blocks/node_accumulator.c
+++ b/quad/src/graph_blocks/node_accumulator.c
@@ -32,10 +32,3 @@ const struct graph_node_type node_accum_type = {
         .type_id = BLOCK_ACCUMULATE
 };
 
-int graph_add_node_accum(struct computation_graph *graph, const char* name) {
-    struct accum_state* node_state = malloc(sizeof(struct accum_state));
-    if (sizeof(struct accum_state) && !node_state) {
-        return -1; // malloc failed
-    }
-    return graph_add_node(graph, name, &node_accum_type, node_state);
-}
diff --git a/quad/src/graph_blocks/node_accumulator.h b/quad/src/graph_blocks/node_accumulator.h
index 43301d6676aa1289a6523da45546ffc88f7cbfe0..99609dd1363d6560a27c8b6afdb2ac90124b05f8 100644
--- a/quad/src/graph_blocks/node_accumulator.h
+++ b/quad/src/graph_blocks/node_accumulator.h
@@ -3,8 +3,6 @@
 #include "computation_graph.h"
 #include "graph_blocks.h"
 
-int graph_add_node_accum(struct computation_graph *graph, const char* name);
-
 extern const struct graph_node_type node_accum_type;
 
 enum graph_node_accum_inputs {
diff --git a/quad/src/graph_blocks/node_add.c b/quad/src/graph_blocks/node_add.c
index 25a780a32681df3d56aec758f9f0527115ee00b7..11f2c07d3d62334e9f7af70214233dffc063e313 100644
--- a/quad/src/graph_blocks/node_add.c
+++ b/quad/src/graph_blocks/node_add.c
@@ -22,6 +22,3 @@ const struct graph_node_type node_add_type = {
         .type_id = BLOCK_ADD
 };
 
-int graph_add_node_add(struct computation_graph *graph, const char* name) {
-    return graph_add_node(graph, name, &node_add_type, NULL);
-}
diff --git a/quad/src/graph_blocks/node_add.h b/quad/src/graph_blocks/node_add.h
index 34b8123638dc1d954ceb0dcd6ceadb959ae6c5c5..6c2925d5f5bf344a6671a942558bc3c597f20578 100644
--- a/quad/src/graph_blocks/node_add.h
+++ b/quad/src/graph_blocks/node_add.h
@@ -3,8 +3,6 @@
 #include "computation_graph.h"
 #include "graph_blocks.h"
 
-int graph_add_node_add(struct computation_graph *graph, const char* name);
-
 extern const struct graph_node_type node_add_type;
 
 enum graph_node_add_inputs {
diff --git a/quad/src/graph_blocks/node_bounds.c b/quad/src/graph_blocks/node_bounds.c
index d1fe264311e1c5223df17f61e7e0330e9466c286..0457b98b1ac2b8a688623a98deceb86885bddab3 100644
--- a/quad/src/graph_blocks/node_bounds.c
+++ b/quad/src/graph_blocks/node_bounds.c
@@ -29,7 +29,3 @@ const struct graph_node_type node_bounds_type = {
         .state_size = 0,
         .type_id = BLOCK_BOUNDS
 };
-
-int graph_add_node_bounds(struct computation_graph *graph, const char* name) {
-    return graph_add_node(graph, name, &node_bounds_type, NULL);
-}
diff --git a/quad/src/graph_blocks/node_bounds.h b/quad/src/graph_blocks/node_bounds.h
index 7eea073fa6000457c30588f251565fcdd3db1a5c..89ad2f04d27c3ea0035842349b003a3fa6073085 100644
--- a/quad/src/graph_blocks/node_bounds.h
+++ b/quad/src/graph_blocks/node_bounds.h
@@ -3,7 +3,6 @@
 #include "computation_graph.h"
 #include "graph_blocks.h"
 
-int graph_add_node_bounds(struct computation_graph *graph, const char* name);
 
 extern const struct graph_node_type node_bounds_type;
 
diff --git a/quad/src/graph_blocks/node_constant.c b/quad/src/graph_blocks/node_constant.c
index 8bd406ad924f3025631c4d251c5d751eddcce90e..13457e77c401886a367c4e694589a9e9dc917793 100644
--- a/quad/src/graph_blocks/node_constant.c
+++ b/quad/src/graph_blocks/node_constant.c
@@ -21,7 +21,3 @@ const struct graph_node_type node_const_type = {
         .state_size = 0,
         .type_id = BLOCK_CONSTANT
 };
-
-int graph_add_node_const(struct computation_graph *graph, const char* name) {
-    return graph_add_node(graph, name, &node_const_type, NULL);
-}
diff --git a/quad/src/graph_blocks/node_constant.h b/quad/src/graph_blocks/node_constant.h
index 417e92da19bbe1170cbec09598e4129d3c4400a6..6a7387c221abe246739440feb1a246fd08ea3ed9 100644
--- a/quad/src/graph_blocks/node_constant.h
+++ b/quad/src/graph_blocks/node_constant.h
@@ -3,8 +3,6 @@
 #include "computation_graph.h"
 #include "graph_blocks.h"
 
-int graph_add_node_const(struct computation_graph *graph, const char* name);
-
 extern const struct graph_node_type node_const_type;
 
 enum graph_node_const_params {
diff --git a/quad/src/graph_blocks/node_gain.c b/quad/src/graph_blocks/node_gain.c
index 3c0ac77ae8a54b0e99b6b767ec85083f3e047e91..ba98bb180270e783e4c9174bf02e94c136c5ed63 100644
--- a/quad/src/graph_blocks/node_gain.c
+++ b/quad/src/graph_blocks/node_gain.c
@@ -21,7 +21,3 @@ const struct graph_node_type node_gain_type = {
         .state_size = 0,
         .type_id = BLOCK_GAIN
 };
-
-int graph_add_node_gain(struct computation_graph *graph, const char* name) {
-    return graph_add_node(graph, name, &node_gain_type, NULL);
-}
diff --git a/quad/src/graph_blocks/node_gain.h b/quad/src/graph_blocks/node_gain.h
index 4a1a3322f65789ffccd6282125181f8deade4200..dfc6a1e2c817eb59300140fd150b451aa110e123 100644
--- a/quad/src/graph_blocks/node_gain.h
+++ b/quad/src/graph_blocks/node_gain.h
@@ -3,8 +3,6 @@
 #include "computation_graph.h"
 #include "graph_blocks.h"
 
-int graph_add_node_gain(struct computation_graph *graph, const char* name);
-
 extern const struct graph_node_type node_gain_type;
 
 enum graph_node_pow_inputs {
diff --git a/quad/src/graph_blocks/node_mixer.c b/quad/src/graph_blocks/node_mixer.c
index fe7d57f276106e7847e5c0b0c0ba0fc92f2ed862..40972ad15521e1ce2d2755eac66afeaf18a0ae35 100644
--- a/quad/src/graph_blocks/node_mixer.c
+++ b/quad/src/graph_blocks/node_mixer.c
@@ -38,7 +38,3 @@ const struct graph_node_type node_mixer_type = {
 		.state_size = 0,
 		.type_id = BLOCK_MIXER
 };
-
-int graph_add_node_mixer(struct computation_graph *graph, const char* name) {
-    return graph_add_node(graph, name, &node_mixer_type, NULL);
-}
diff --git a/quad/src/graph_blocks/node_mixer.h b/quad/src/graph_blocks/node_mixer.h
index 584538f0242677428632b681b2f0fe064e789abc..374a88be39e409806c6c7a43d95cb5e5b35e7849 100644
--- a/quad/src/graph_blocks/node_mixer.h
+++ b/quad/src/graph_blocks/node_mixer.h
@@ -3,8 +3,6 @@
 #include "computation_graph.h"
 #include "graph_blocks.h"
 
-int graph_add_node_mixer(struct computation_graph *graph, const char* name);
-
 extern const struct graph_node_type node_mixer_type;
 
 enum graph_node_mixer_inputs {
diff --git a/quad/src/graph_blocks/node_mult.c b/quad/src/graph_blocks/node_mult.c
index 427b9e74d071b45bbd5c7ef2437647f13aeb5b5c..225bec6b707764472e54b5783915292f8a2a69e9 100644
--- a/quad/src/graph_blocks/node_mult.c
+++ b/quad/src/graph_blocks/node_mult.c
@@ -21,7 +21,3 @@ const struct graph_node_type node_mult_type = {
         .state_size = 0,
         .type_id = BLOCK_MULT
 };
-
-int graph_add_node_mult(struct computation_graph *graph, const char* name) {
-    return graph_add_node(graph, name, &node_mult_type, NULL);
-}
diff --git a/quad/src/graph_blocks/node_mult.h b/quad/src/graph_blocks/node_mult.h
index f86f7ee0ab88c64dc1f3241615ac06d66fc3d925..471de30f51241fa5ef1a55eeefd0ef879e92a3ef 100644
--- a/quad/src/graph_blocks/node_mult.h
+++ b/quad/src/graph_blocks/node_mult.h
@@ -3,8 +3,6 @@
 #include "computation_graph.h"
 #include "graph_blocks.h"
 
-int graph_add_node_mult(struct computation_graph *graph, const char* name);
-
 extern const struct graph_node_type node_mult_type;
 
 enum graph_node_mult_inputs {
diff --git a/quad/src/graph_blocks/node_pid.c b/quad/src/graph_blocks/node_pid.c
index 74ee28ab93f7817bdd9e59c61136b6b67d03c525..d8f555be59d6751bc4a97fb08fc8024e13faadb2 100644
--- a/quad/src/graph_blocks/node_pid.c
+++ b/quad/src/graph_blocks/node_pid.c
@@ -101,11 +101,3 @@ const struct graph_node_type node_pid_type = {
         .state_size = sizeof(struct pid_node_state),
         .type_id = BLOCK_PID
 };
-
-int graph_add_node_pid(struct computation_graph *graph, const char* name) {
-    struct pid_node_state* node_state = malloc(sizeof(struct pid_node_state));
-    if (sizeof(struct pid_node_state) && !node_state) {
-        return -1; // malloc failed
-    }
-    return graph_add_node(graph, name, &node_pid_type, node_state);
-}
diff --git a/quad/src/graph_blocks/node_pid.h b/quad/src/graph_blocks/node_pid.h
index 694f7a143d5141d7bbcaf951302fee187f56ab15..2570a9f79568a8d699182c7d9d7166e31e9bfe3b 100644
--- a/quad/src/graph_blocks/node_pid.h
+++ b/quad/src/graph_blocks/node_pid.h
@@ -3,8 +3,6 @@
 #include "computation_graph.h"
 #include "graph_blocks.h"
 
-int graph_add_node_pid(struct computation_graph *graph, const char* name);
-
 extern const struct graph_node_type node_pid_type;
 
 enum graph_node_pid_inputs {
diff --git a/quad/src/graph_blocks/node_pow.c b/quad/src/graph_blocks/node_pow.c
index e4fe0f25c2dd323085110869e3f1191217914d40..fc03f1c501d33ecfc9c2a3d8dbf4c5882e5d7dc7 100644
--- a/quad/src/graph_blocks/node_pow.c
+++ b/quad/src/graph_blocks/node_pow.c
@@ -21,7 +21,3 @@ const struct graph_node_type node_pow_type = {
         .reset = reset,
         .state_size = 0,
 };
-
-int graph_add_node_pow(struct computation_graph *graph, const char* name) {
-    return graph_add_node(graph, name, &node_pow_type, NULL);
-}
diff --git a/quad/src/graph_blocks/node_pow.h b/quad/src/graph_blocks/node_pow.h
index 56a73d3d05fe12f22b16ae248a22347647f95ae0..33c0a5c1e638c50aa7c3fc23df0680052ec5484e 100644
--- a/quad/src/graph_blocks/node_pow.h
+++ b/quad/src/graph_blocks/node_pow.h
@@ -2,8 +2,6 @@
 #define __NODE_POW_H__
 #include "computation_graph.h"
 
-int graph_add_node_pow(struct computation_graph *graph, const char* name);
-
 extern const struct graph_node_type node_pow_type;
 
 enum graph_node_pow_inputs {
diff --git a/quad/src/graph_blocks/node_yaw_rot.c b/quad/src/graph_blocks/node_yaw_rot.c
new file mode 100644
index 0000000000000000000000000000000000000000..fe4a6a0c457d4c9b0a03045cc09565c9c70a796a
--- /dev/null
+++ b/quad/src/graph_blocks/node_yaw_rot.c
@@ -0,0 +1,40 @@
+#include "node_yaw_rot.h"
+#include <stdlib.h>
+
+static void rotate_yaw(void *state, const double* params, const double *inputs, double *outputs) {
+	// Psuedo-Nonlinear Extension for determining local x/y position based on yaw angle
+	// provided by Matt Rich
+	//
+	// local x/y/z is the moving frame of reference on the quad that we are transforming so we can assume yaw angle is 0 (well enough)
+	// 		for the autonomous position controllers
+	//
+	// camera given x/y/z is the inertia frame of reference (the global coordinates)
+	//
+	// |local x|	|cos(yaw angle)  -sin(yaw angle)  0| |camera given x|
+	// |local y|  = |sin(yaw angle)   cos(yaw angle)  0| |camera given y|
+	// |local z|	|       0               0         1| |camera given z|
+
+    outputs[ROT_OUT_X] =
+        inputs[ROT_CUR_X] * cos(inputs[ROT_YAW]) + inputs[ROT_CUR_Y] * -sin(inputs[ROT_YAW]);
+
+	outputs[ROT_OUT_Y] =
+			inputs[ROT_CUR_X] * sin(inputs[ROT_YAW]) + inputs[ROT_CUR_Y] * cos(inputs[ROT_YAW]);
+
+}
+static void reset(void *state) {}
+
+static const char* const in_names[3] = {"Current Yaw", "X Position", "Y Position"};
+static const char* const out_names[2] = {"Rotated X", "Rotated Y"};
+static const char* const param_names[1] = {"Error if you see this"};
+const struct graph_node_type node_yaw_rot_type = {
+        .input_names = in_names,
+        .output_names = out_names,
+        .param_names = param_names,
+        .n_inputs = 3,
+        .n_outputs = 2,
+        .n_params = 0,
+        .execute = rotate_yaw,
+        .reset = reset,
+        .state_size = 0,
+        .type_id = BLOCK_YAW_ROT
+};
diff --git a/quad/src/graph_blocks/node_yaw_rot.h b/quad/src/graph_blocks/node_yaw_rot.h
new file mode 100644
index 0000000000000000000000000000000000000000..cfc9f41bb067eaac72f73de53175cd4b60ab99b0
--- /dev/null
+++ b/quad/src/graph_blocks/node_yaw_rot.h
@@ -0,0 +1,18 @@
+#ifndef __NODE_ROTATE_H__
+#define __NODE_ROTATE_H__
+#include "computation_graph.h"
+#include "graph_blocks.h"
+
+extern const struct graph_node_type node_yaw_rot_type;
+
+enum graph_node_yaw_rot_inputs {
+    ROT_YAW, // Amount of current yaw
+    ROT_CUR_X, // Input X position
+    ROT_CUR_Y // Input Y position
+};
+
+enum graph_node_yaw_rot_outputs {
+    ROT_OUT_X, // Rotated X position
+    ROT_OUT_Y // Rotated Y position
+};
+#endif // __NODE_ROTATE_H__
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index dd7198c44929bf58845569e22457fbfdcae92498..7d867cbfc2d498e769cccd1463f1d441710cc3e7 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -23,8 +23,8 @@ 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->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->pitch_pid, PID_SETPOINT, ps->yaw_correction, ROT_OUT_X);
+	graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->yaw_correction, ROT_OUT_Y);
 	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);
 }
@@ -96,6 +96,8 @@ int control_algorithm_init(parameter_t * ps)
     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");
+        // Converts global X/Y to local X/Y
+    ps->yaw_correction = graph_add_defined_block(graph, BLOCK_YAW_ROT, "Yaw Correction");
 
     ps->mixer = graph_add_defined_block(graph, BLOCK_MIXER, "Signal Mixer");
 
@@ -143,6 +145,12 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->x_vel_pid, PID_SETPOINT, ps->x_vel_clamp, BOUNDS_OUT);
     graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
 
+    // X/Y global to local conversion
+    graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->cur_yaw, CONST_VAL);
+    graph_set_source(graph, ps->yaw_correction, ROT_CUR_X, ps->x_vel_pid, PID_CORRECTION);
+    graph_set_source(graph, ps->yaw_correction, ROT_CUR_Y, ps->y_vel_pid, PID_CORRECTION);
+    
+
     // Y velocity PID
         // Use a PID block to compute the derivative
     graph_set_param_val(graph, ps->y_vel, PID_KD, -1);
diff --git a/quad/src/quad_app/sensor.c b/quad/src/quad_app/sensor.c
index 21ee86f3c31246439262a0f20a49b232b96d3ca5..2d9396de9cf254be598bd2d2cbea42bf47528c31 100644
--- a/quad/src/quad_app/sensor.c
+++ b/quad/src/quad_app/sensor.c
@@ -67,6 +67,7 @@ int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t
 //
 //	/////////// end testing
 
+	int status = 0;
 
 	// the the sensor board and fill in the readings into the GAM struct
 	iic0_mpu9150_read_gam(&(raw_sensor_struct->gam));
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index 5d8a714e09db5b17a98ca9e663404435c51b4e2c..5e3ca9f2ff4484625da6df72009e77ce44ade924 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -44,28 +44,6 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 	// copy currentQuadPosition and trimmedRCValues from raw_sensor_struct to sensor_struct
 	deep_copy_Qpos(&(sensor_struct->currentQuadPosition), &(raw_sensor_struct->currentQuadPosition));
 
-	// Psuedo-Nonlinear Extension for determining local x/y position based on yaw angle
-	// provided by Matt Rich
-	//
-	// local x/y/z is the moving frame of reference on the quad that we are transforming so we can assume yaw angle is 0 (well enough)
-	// 		for the autonomous position controllers
-	//
-	// camera given x/y/z is the inertia frame of reference (the global coordinates)
-	//
-	// |local x|	|cos(yaw angle)  -sin(yaw angle)  0| |camera given x|
-	// |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 =
-			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 =
-			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
 	/////////////////////////////////////////////////////////////////////////
 	// | Phi_d   |   |  1  sin(Phi)tan(theta)    cos(Phi)tan(theta) |  | p |
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index 80e676956b648a9cb9bd6e528ca96dc147d25c3a..b7e398b6bb4cd2222f384c00b2fc05df9f6504a4 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -369,6 +369,8 @@ typedef struct parameter_t {
 	int y_vel_clamp;
 	int vel_x_gain;
 	int vel_y_gain;
+	// Sensor processing
+	int yaw_correction;
 } parameter_t;
 
 /**