diff --git a/include/imu.h b/include/imu.h
index a682989..1aa8a18 100644
--- a/include/imu.h
+++ b/include/imu.h
@@ -16,16 +16,24 @@ bool imuIsEnabled();
 void imuInit(uint8_t addr, TwoWire *theWire);
 void imuCalibrate(unsigned long calibrationTime);
 
-bool imuPeriodic();
+void imuPeriodic();
+bool imuDataReady();
 
-float gyroGetAngleX();
-float gyroGetRateX();
+float imuGetAccelX();
+float imuGetAccelY();
+float imuGetAccelZ();
 
-float gyroGetAngleY();
-float gyroGetRateY();
+float imuGetGyroRateX();
+float imuGetGyroRateY();
+float imuGetGyroRateZ();
 
-float gyroGetAngleZ();
-float gyroGetRateZ();
+float imuGetRoll();
+float imuGetPitch();
+float imuGetYaw();
+
+void imuResetRoll();
+void imuResetPitch();
+void imuResetYaw();
 
 void gyroReset();
 
diff --git a/platformio.ini b/platformio.ini
index 82cab68..55c7175 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -12,4 +12,5 @@ lib_deps =
     Wire
     adafruit/Adafruit LSM6DS@^4.7.0
     adafruit/Adafruit BusIO@^1.14.1
-    adafruit/Adafruit Unified Sensor@^1.1.9
\ No newline at end of file
+    adafruit/Adafruit Unified Sensor@^1.1.9
+    arduino-libraries/Madgwick@^1.2.0
\ No newline at end of file
diff --git a/src/imu.cpp b/src/imu.cpp
index 59827ed..fd99fdc 100644
--- a/src/imu.cpp
+++ b/src/imu.cpp
@@ -1,7 +1,11 @@
 #include "imu.h"
 
+#include <MadgwickAHRS.h>
+
 #define IMU_DEFAULT_CALIBRATION_TIME_MS 3000
 
+#define IMU_MADGWICK_LOOP_FREQ_HZ 25
+
 namespace xrp {
 
 unsigned long _imuUpdatePeriod = 1000 / IMU_UPDATE_RATE_HZ;
@@ -12,19 +16,37 @@ bool _imuEnabled = false;
 unsigned long _lastIMUUpdateTime = 0;
 bool _imuOnePassComplete = false;
 
-float _gyroOffsetsDegPerSec[3] = {0, 0, 0};
-float _gyroRatesDegPerSec[3] = {0, 0, 0};
+float _accelOffsetsG[3] = {0, 0, 0};
+float _accelG[3] = {0, 0, 0};
+
+float _gyroOffsetsDPS[3] = {0, 0, 0};
+float _gyroRatesDPS[3] = {0, 0, 0};
 float _gyroAnglesDeg[3] = {0, 0, 0};
 
+float _ahrsOffsets[3] = {0, 0, 0};
+
+Madgwick _ahrsFilter;
+bool _filterStarted = false;
+unsigned long _microsPerReading, _microsPrevious;
+
 float _radToDeg(float angleRad) {
   return angleRad * 180.0 / PI;
 }
 
+float _accelToG(float accelMS2) {
+  return accelMS2 / 9.80665;
+}
+
 bool imuIsReady() {
     return _imuReady;
 }
 
 void imuSetEnabled(bool enabled) {
+  // If going from false to true, reset the gyro
+  if (!_imuEnabled && enabled) {
+    gyroReset();
+  }
+
   _imuEnabled = enabled;
   Serial.printf("[IMU] %s\n", enabled ? "Enabling" : "Disabling");
 }
@@ -96,6 +118,8 @@ void imuCalibrate(unsigned long calibrationTimeMs) {
   Serial.printf("[IMU] Beginning calibration. Running for %u ms\n", calibrationTimeMs);
   
   float gyroAvgValues[3] = {0, 0, 0};
+  float accelAvgValues[3] = {0, 0, 0};
+
   int numVals = 0;
 
   bool ledBlinkState = true;
@@ -119,7 +143,13 @@ void imuCalibrate(unsigned long calibrationTimeMs) {
     sensors_event_t temp;
 
     _lsm6.getEvent(&accel, &gyro, &temp);
+    
+    // Accelerometer averages
+    accelAvgValues[0] += _accelToG(accel.acceleration.x);
+    accelAvgValues[1] += _accelToG(accel.acceleration.y);
+    accelAvgValues[2] += _accelToG(accel.acceleration.z);
 
+    // Gyro averages
     gyroAvgValues[0] += _radToDeg(gyro.gyro.x);
     gyroAvgValues[1] += _radToDeg(gyro.gyro.y);
     gyroAvgValues[2] += _radToDeg(gyro.gyro.z);
@@ -128,91 +158,252 @@ void imuCalibrate(unsigned long calibrationTimeMs) {
     delay(loopDelayTime);
   }
 
-  _gyroOffsetsDegPerSec[0] = gyroAvgValues[0] / numVals;
-  _gyroOffsetsDegPerSec[1] = gyroAvgValues[1] / numVals;
-  _gyroOffsetsDegPerSec[2] = gyroAvgValues[2] / numVals;
+  _accelOffsetsG[0] = accelAvgValues[0] / numVals;
+  _accelOffsetsG[1] = accelAvgValues[1] / numVals;
+  _accelOffsetsG[2] = accelAvgValues[2] / numVals;
 
-  Serial.printf("[IMU] Offsets: X(%f) Y(%f) Z(%f)\n",
-      _gyroOffsetsDegPerSec[0],
-      _gyroOffsetsDegPerSec[1],
-      _gyroOffsetsDegPerSec[2]);
+  _gyroOffsetsDPS[0] = gyroAvgValues[0] / numVals;
+  _gyroOffsetsDPS[1] = gyroAvgValues[1] / numVals;
+  _gyroOffsetsDPS[2] = gyroAvgValues[2] / numVals;
+
+  // Remove 1G from the vertical axis (assumed to be Z)
+  _accelOffsetsG[2] -= 1.0;
+
+  Serial.printf("[IMU] Gyro Offsets(dps): X(%f) Y(%f) Z(%f), Accel Offsets(g): X(%f) Y(%f) Z(%f)\n",
+      _gyroOffsetsDPS[0],
+      _gyroOffsetsDPS[1],
+      _gyroOffsetsDPS[2],
+      _accelOffsetsG[0],
+      _accelOffsetsG[1],
+      _accelOffsetsG[2]);
   Serial.println("[IMU] Calibration Complete");
 
   digitalWrite(LED_BUILTIN, LOW);
 }
 
-bool imuPeriodic() {
+unsigned long _imuLoopTime = 0;
+int _imuLoopCount = 0;
+
+void imuPeriodic() {
+  // Initialize the filter if this is the first time we are running through the periodic
+  if (!_filterStarted) {
+    Serial.printf("[IMU] Starting Madgwick filter at %u hz\n", IMU_MADGWICK_LOOP_FREQ_HZ);
+    _microsPerReading = 1000000 / IMU_MADGWICK_LOOP_FREQ_HZ;
+    _microsPrevious = micros();
+    _ahrsFilter.begin(IMU_MADGWICK_LOOP_FREQ_HZ);
+    _filterStarted = true;
+    return;
+  }
+
+  unsigned long microsNow = micros();
+  if (microsNow - _microsPrevious >= _microsPerReading) {
+    // Read data
+    sensors_event_t accel;
+    sensors_event_t gyro;
+    sensors_event_t temp;
+
+    _lsm6.getEvent(&accel, &gyro, &temp);
+
+    _gyroRatesDPS[0] = _radToDeg(gyro.gyro.x) - _gyroOffsetsDPS[0];
+    _gyroRatesDPS[1] = _radToDeg(gyro.gyro.y) - _gyroOffsetsDPS[1];
+    _gyroRatesDPS[2] = _radToDeg(gyro.gyro.z) - _gyroOffsetsDPS[2];
+
+    _accelG[0] = _accelToG(accel.acceleration.x) - _accelOffsetsG[0];
+    _accelG[1] = _accelToG(accel.acceleration.y) - _accelOffsetsG[1];
+    _accelG[2] = _accelToG(accel.acceleration.z) - _accelOffsetsG[2];
+
+    // Update the filter, which will compute orientation
+    _ahrsFilter.updateIMU(_gyroRatesDPS[0], _gyroRatesDPS[1], _gyroRatesDPS[2], _accelG[0], _accelG[1], _accelG[2]);
+
+    // Increment the previous time so that we keep proper pace
+    _microsPrevious = _microsPrevious + _microsPerReading;
+
+    // Stats
+    _imuLoopTime += micros() - microsNow;
+    _imuLoopCount++;
+
+    if (_imuLoopCount > 100) {
+      Serial.printf("[IMU] Avg AHRS Update Time: %u us\n", _imuLoopTime / _imuLoopCount);
+      _imuLoopCount = 0;
+      _imuLoopTime = 0;
+    }
+  }
+}
+
+/**
+ * Determine if we have data ready to send upstream
+ */
+bool imuDataReady() {
   if (!_imuReady) return false;
   if (!_imuEnabled) return false;
 
   unsigned long currTime = millis();
   unsigned long dt = currTime - _lastIMUUpdateTime;
 
-  // Send the IMU data at the specified IMU_UPDATE_RATE_HZ
   if (dt < _imuUpdatePeriod) return false;
 
-  // Get IMU data
-  sensors_event_t accel;
-  sensors_event_t gyro;
-  sensors_event_t temp;
-
-  _lsm6.getEvent(&accel, &gyro, &temp);
-
-  float rateX = _radToDeg(gyro.gyro.x);
-  float rateY = _radToDeg(gyro.gyro.y);
-  float rateZ = _radToDeg(gyro.gyro.z);
-  _gyroRatesDegPerSec[0] = rateX - _gyroOffsetsDegPerSec[0];
-  _gyroRatesDegPerSec[1] = rateY - _gyroOffsetsDegPerSec[1];
-  _gyroRatesDegPerSec[2] = rateZ - _gyroOffsetsDegPerSec[2];
-
-  // We can't integrate with only a single value, so bail out
-  if (!_imuOnePassComplete) {
-    _imuOnePassComplete = true;
-    _lastIMUUpdateTime = currTime;
-    return false;
-  }
+  _lastIMUUpdateTime = currTime;
+  return true;
+}
+
+// bool imuPeriodic() {
+//   if (!_imuReady) return false;
+//   if (!_imuEnabled) return false;
+
+//   unsigned long currTime = millis();
+//   unsigned long dt = currTime - _lastIMUUpdateTime;
+
+//   // Send the IMU data at the specified IMU_UPDATE_RATE_HZ
+//   if (dt < _imuUpdatePeriod) return false;
+
+//   // Get IMU data
+//   sensors_event_t accel;
+//   sensors_event_t gyro;
+//   sensors_event_t temp;
+
+//   _lsm6.getEvent(&accel, &gyro, &temp);
+
+//   float rateX = _radToDeg(gyro.gyro.x);
+//   float rateY = _radToDeg(gyro.gyro.y);
+//   float rateZ = _radToDeg(gyro.gyro.z);
+//   _gyroRatesDPS[0] = rateX - _gyroOffsetsDPS[0];
+//   _gyroRatesDPS[1] = rateY - _gyroOffsetsDPS[1];
+//   _gyroRatesDPS[2] = rateZ - _gyroOffsetsDPS[2];
+
+//   // We can't integrate with only a single value, so bail out
+//   if (!_imuOnePassComplete) {
+//     _imuOnePassComplete = true;
+//     _lastIMUUpdateTime = currTime;
+//     return false;
+//   }
   
-  float dtInSeconds = dt / 1000.0;
+//   float dtInSeconds = dt / 1000.0;
+
+//   for (int i = 0; i < 3; i++) {
+//     _gyroAnglesDeg[i] = _gyroAnglesDeg[i] + (_gyroRatesDPS[i] * dtInSeconds);
+//   }
+
+//   _lastIMUUpdateTime = currTime;
+//   return true;
+// }
+
+// ===============================
+// AHRS Values
+// ===============================
+
+/**
+ * Get acceleration in the X axis
+ * 
+ * @return Acceleration in X (in G)
+ */
+float imuGetAccelX() {
+  return _accelG[0];
+}
 
-  for (int i = 0; i < 3; i++) {
-    _gyroAnglesDeg[i] = _gyroAnglesDeg[i] + (_gyroRatesDegPerSec[i] * dtInSeconds);
-  }
+/**
+ * Get acceleration in the Y axis
+ * 
+ * @return Acceleration in Y (in G)
+ */
+float imuGetAccelY() {
+  return _accelG[1];
+}
 
-  _lastIMUUpdateTime = currTime;
-  return true;
+/**
+ * Get acceleration in the Z axis
+ * 
+ * @return Acceleration in Z (in G)
+ */
+float imuGetAccelZ() {
+  return _accelG[2];
 }
 
-float gyroGetAngleX() {
-  return _gyroAnglesDeg[0];
+/**
+ * Get gyro rate in the X axis
+ * 
+ * @return Gyro rate in X (in DPS)
+ */
+float imuGetGyroRateX() {
+  return _gyroRatesDPS[0];
 }
 
-float gyroGetRateX() {
-  return _gyroRatesDegPerSec[0];
+/**
+ * Get gyro rate in the Y axis
+ * 
+ * @return Gyro rate in Y (in DPS)
+ */
+float imuGetGyroRateY() {
+  return _gyroRatesDPS[1];
 }
 
-float gyroGetAngleY() {
-  return _gyroAnglesDeg[1];
+/**
+ * Get gyro rate in the Z axis
+ * 
+ * @return Gyro rate in Z (in DPS)
+ */
+float imuGetGyroRateZ() {
+  return _gyroRatesDPS[2];
 }
 
-float gyroGetRateY() {
-  return _gyroRatesDegPerSec[1];
+/**
+ * Get current roll angle
+ * 
+ * @return Current roll angle (in degrees)
+ */
+float imuGetRoll() {
+  return _ahrsFilter.getRoll() - _ahrsOffsets[0];
 }
 
-float gyroGetAngleZ() {
-  return _gyroAnglesDeg[2];
+/**
+ * Get current pitch angle
+ * 
+ * @return Current pitch angle (in degrees)
+ */
+float imuGetPitch() {
+  return _ahrsFilter.getPitch() - _ahrsOffsets[1];
 }
 
-float gyroGetRateZ() {
-  return _gyroRatesDegPerSec[2];
+/**
+ * Get current yaw angle
+ * 
+ * @return Current yaw angle (in degrees)
+ */
+float imuGetYaw() {
+  return _ahrsFilter.getYaw() - _ahrsOffsets[2];
 }
 
-void gyroReset() {
-  for (int i = 0; i < 3; i++) {
-    _gyroRatesDegPerSec[i] = 0;
-    _gyroAnglesDeg[i] = 0;
-  }
+/**
+ * Reset the roll angle.
+ * 
+ * The AHRS filter always runs, so this basically sets an offset value
+ */
+void imuResetRoll() {
+  _ahrsOffsets[0] = _ahrsFilter.getRoll();
+}
+
+/**
+ * Reset the pitch angle.
+ * 
+ * The AHRS filter always runs, so this basically sets an offset value
+ */
+void imuResetPitch() {
+  _ahrsOffsets[1] = _ahrsFilter.getPitch();
+}
 
-  _imuOnePassComplete = false;
+/**
+ * Reset the yaw angle.
+ * 
+ * The AHRS filter always runs, so this basically sets an offset value
+ */
+void imuResetYaw() {
+  _ahrsOffsets[2] = _ahrsFilter.getYaw();
+}
+
+void gyroReset() {
+  Serial.println("[IMU] Resetting Gyro");
+  imuResetRoll();
+  imuResetPitch();
+  imuResetYaw();
 }
 
 } // namespace xrp
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 14cf6b7..345ca8a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -160,6 +160,9 @@ void loop() {
   // webServer.handleClient();
   wsServer.loop();
 
+  // TODO always run the IMU periodic routine
+  xrp::imuPeriodic();
+
   // Disable the robot when we no longer have a connection
   int numConnectedClients = wsServer.connectedClients();
   if (lastCheckedNumClients > 0 && numConnectedClients == 0) {
@@ -188,11 +191,11 @@ void loop() {
     }
 
     // Read Gyro Data
-    if (xrp::imuPeriodic()) {
-      float rateZ = xrp::gyroGetRateZ();
-      float angleZ = xrp::gyroGetAngleZ();
+    if (xrp::imuDataReady()) {
+      float rateZ = xrp::imuGetGyroRateZ();
+      float yawAngle = xrp::imuGetYaw();
       
-      sendMessage(wpilibws::makeGyroSingleMessage(wpilibws::AXIS_Z, rateZ, angleZ));
+      sendMessage(wpilibws::makeGyroSingleMessage(wpilibws::AXIS_Z, rateZ, yawAngle));
     }
 
   }