diff --git a/adafruit-AHRS.cpp b/adafruit-AHRS.cpp
index d2e97d6..65c45dd 100644
--- a/adafruit-AHRS.cpp
+++ b/adafruit-AHRS.cpp
@@ -2,10 +2,13 @@
 */
 
 // VERY IMPORTANT!
-//These are the previously determined offsets and scale factors for accelerometer and magnetometer, using MPU9250_cal and Magneto
-//The compass will NOT work well or at all if these are not correct
+//These are the previously determined offsets and scale factors for the gyroscope, accelerometer and magnetometer, using the mega
+//The AHRS will not work without the correct calibration parameters
 
-//Accel scale 16457.0 to normalize
+//Gyroscope calibration parameters
+
+G_offset[3] = {75, 31, 142};
+//Accelerometer calibration parameters
 float A_B[3]
 { -133.33,   72.29, -291.92};
 
@@ -15,7 +18,7 @@ float A_Ainv[3][3]
   {  0.00023,  0.00263,  0.99905}
 };
 
-//Mag scale 3746.0 to normalize
+//Magnetometer calibration parameters
 float M_B[3]
 { -922.31, 2199.41,  373.17};
 
@@ -25,24 +28,25 @@ float M_Ainv[3][3]
   { -0.01714,  0.00644,  1.07005}
 };
 
-
-
-
-
-// TILT COMPENSATION CODE
 // local magnetic declination in degrees in Hamburg
 float declination = +4.08;
 
-/*
-  This tilt-compensated code assumes that the Adafruit LSM9DS1 sensor board is oriented with Y pointing
-  to the North, X pointing West, and Z pointing up.
-  The code compensates for tilts of up to 90 degrees away from horizontal.
-  Facing vector p is the direction of travel and allows reassigning these directions.
-  It should be defined as pointing forward,
-  parallel to the ground, with coordinates {X, Y, Z} (in magnetometer frame of reference).
-*/
+
 float p[] = {0, 1, 0};  //Y marking on sensor board points toward yaw = 0
 
+
+// MAHONY PART
+
+// These are the free parameters in the Mahony filter and fusion scheme,
+// Kp for proportional feedback, Ki for integral
+// Kp is not yet optimized. Ki is not used.
+#define Kp 50
+static float q[4] = {1.0, 0.0, 0.0, 0.0};
+static float yaw, pitch, roll; //Euler angle output
+
+
+
+
 // Returns a heading (in degrees) given an acceleration vector a due to gravity, a magnetic vector m, and a facing vector p.
 int get_heading(float acc[3], float mag[3], float p[3])
 {
@@ -114,4 +118,12 @@ void vector_normalize(float a[3])
   a[2] /= mag;
 }
 
-// END OF TILT COMPENSATION CODE
+/*
+  This tilt-compensated code assumes that the Adafruit LSM9DS1 sensor board is oriented with Y pointing
+  to the North, X pointing West, and Z pointing up.
+  The code compensates for tilts of up to 90 degrees away from horizontal.
+  Facing vector p is the direction of travel and allows reassigning these directions.
+  It should be defined as pointing forward,
+  parallel to the ground, with coordinates {X, Y, Z} (in magnetometer frame of reference).
+*/
+