diff --git a/frtos/config/magnetometer_config.h b/frtos/config/magnetometer_config.h
index 5acb030..1128376 100644
--- a/frtos/config/magnetometer_config.h
+++ b/frtos/config/magnetometer_config.h
@@ -7,8 +7,13 @@
 
 #define DIRECTION_READ_DELAY                    ( 100 )
 
-#define ALPHA                                   ( 0.0f ) // Complementary
-                                                          // Filter Constant
+#define NUM_READINGS                            ( 10 ) // Number of readings to
+                                                        // take before
+                                                        // calculating
+                                                        // direction
+
+//#define ALPHA                                   ( 0.1f ) // Low Pass Filter
+                                                          // Coefficient
 
 // LSM303DLHC temperature compensation coefficients
 #define SCALE_Z                                 ( 1.0f ) // Scale for Z-axis
diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h
index b8addee..2cabc97 100644
--- a/frtos/magnetometer/magnetometer_direction.h
+++ b/frtos/magnetometer/magnetometer_direction.h
@@ -65,10 +65,10 @@ calculate_yaw_magnetometer(int16_t magnetometer[3]) {
  * @param yaw_mag   Yaw calculated from Magnetometer Data
  * @return yaw      Yaw calculated from Complementary Filter
  */
-static inline float
-calculate_yaw_complementary(float yaw_acc, float yaw_mag) {
-    return ALPHA * yaw_acc + (1 - ALPHA) * yaw_mag;
-}
+//static inline float
+//calculate_yaw_complementary(float yaw_acc, float yaw_mag) {
+//    return ALPHA * yaw_acc + (1 - ALPHA) * yaw_mag;
+//}
 
 /**
  * @brief Compensate the magnetometer readings for temperature
@@ -220,9 +220,7 @@ update_orientation_data(float roll, float pitch, float yaw,
  * @param magnetometer  Magnetometer Data
  */
 static void
-read_direction(int16_t acceleration[3],
-               int16_t magnetometer[3],
-               int16_t temperature[1]) {
+read_direction(int16_t acceleration[3], int16_t magnetometer[3]) {
 
     float roll = calculate_roll(acceleration);
     float pitch = calculate_pitch(acceleration);
@@ -260,7 +258,8 @@ read_direction(int16_t acceleration[3],
  * @param params
  */
 void print_orientation_data() {
-    printf("Roll: %f, Pitch: %f, Yaw: %f\n",
+//    printf("Roll: %f, Pitch: %f, Yaw: %f\n",
+           printf("%f %f %f\n",
            g_direction.roll,
            g_direction.pitch,
            g_direction.yaw
@@ -331,16 +330,16 @@ void updateDirection() {
     read_accelerometer(accelerometer);
     read_temperature(temperature);
 
-    read_direction(accelerometer, magnetometer, temperature);
+    read_direction(accelerometer, magnetometer);
 
     // Temperature in degrees Celsius
-    printf("Temperature: %d\n", temperature[0]);
+//    printf("Temperature: %d\n", temperature[0]);
 
     print_orientation_data();
 
-    printf("Direction: ");
+//    printf("Direction: ");
 
-    print_direction(g_direction.orientation);
+//    print_direction(g_direction.orientation);
 
     switch (g_direction.orientation)
     {
diff --git a/frtos/magnetometer/magnetometer_read.h b/frtos/magnetometer/magnetometer_read.h
index fde4b0b..36f3f35 100644
--- a/frtos/magnetometer/magnetometer_read.h
+++ b/frtos/magnetometer/magnetometer_read.h
@@ -58,25 +58,34 @@ read_accelerometer(int16_t accelerometer[3]) {
 }
 
 /**
- * @brief Read Magnetometer Data
+ * @brief Read Magnetometer Data with Moving Average
  * @param magnetometer  Magnetometer Data
  */
 static inline void
 read_magnetometer(int16_t magnetometer[3]) {
     uint8_t buffer[6];
+    int32_t xMagFiltered = 0;
+    int32_t yMagFiltered = 0;
+    int32_t zMagFiltered = 0;
 
-    buffer[0] = read_data(MAG_ADDR, LSM303_OUT_X_H_M);
-    buffer[1] = read_data(MAG_ADDR, LSM303_OUT_X_L_M);
-    buffer[2] = read_data(MAG_ADDR, LSM303_OUT_Y_H_M);
-    buffer[3] = read_data(MAG_ADDR, LSM303_OUT_Y_L_M);
-    buffer[4] = read_data(MAG_ADDR, LSM303_OUT_Z_H_M);
-    buffer[5] = read_data(MAG_ADDR, LSM303_OUT_Z_L_M);
+    for (int i = 0; i < NUM_READINGS; i++) {
+        buffer[0] = read_data(MAG_ADDR, LSM303_OUT_X_H_M);
+        buffer[1] = read_data(MAG_ADDR, LSM303_OUT_X_L_M);
+        buffer[2] = read_data(MAG_ADDR, LSM303_OUT_Y_H_M);
+        buffer[3] = read_data(MAG_ADDR, LSM303_OUT_Y_L_M);
+        buffer[4] = read_data(MAG_ADDR, LSM303_OUT_Z_H_M);
+        buffer[5] = read_data(MAG_ADDR, LSM303_OUT_Z_L_M);
 
-    magnetometer[0] = (int16_t) (buffer[0] << 8 | buffer[1]); //xMag
+        // Update the cumulative sum of the magnetometer data
+        xMagFiltered += (int16_t)(buffer[0] << 8 | buffer[1]);
+        yMagFiltered += (int16_t)(buffer[2] << 8 | buffer[3]);
+        zMagFiltered += (int16_t)(buffer[4] << 8 | buffer[5]);
+    }
 
-    magnetometer[1] = (int16_t) (buffer[2] << 8 | buffer[3]); //yMag
-
-    magnetometer[2] = (int16_t) (buffer[4] << 8 | buffer[5]); //zMag
+    // Calculate the moving average
+    magnetometer[0] = xMagFiltered / NUM_READINGS;
+    magnetometer[1] = yMagFiltered / NUM_READINGS;
+    magnetometer[2] = zMagFiltered / NUM_READINGS;
 }
 
 /**