diff --git a/lib/LSM6/src/LSM6.cpp b/lib/LSM6/src/LSM6.cpp
index ac124c5..346f77d 100644
--- a/lib/LSM6/src/LSM6.cpp
+++ b/lib/LSM6/src/LSM6.cpp
@@ -29,7 +29,7 @@ LSM6::LSM6(void)
 bool LSM6::checkForNewData(void)
 {
   bool retVal = false;
-  if(getStatus() & 0x01)
+  if(getStatus() & 0x02)
   {
     read();
 
@@ -228,7 +228,7 @@ void LSM6::enableDefault(void)
   {
     // Set the gyro full scale and data rate
     setFullScaleGyro(GYRO_FS245);
-    setGyroDataOutputRate(ODR13);
+    setGyroDataOutputRate(ODR104);
 
     // Set the accelerometer full scale and data rate
     setFullScaleAcc(ACC_FS2);
diff --git a/lib/LSM6/src/LSM6.h b/lib/LSM6/src/LSM6.h
index abff397..4c4bcaa 100644
--- a/lib/LSM6/src/LSM6.h
+++ b/lib/LSM6/src/LSM6.h
@@ -141,6 +141,12 @@ public:
 
     vector<float> updateGyroBias(void) 
     {
+      if (initialMeasurement) {
+          gyroBias.z = g.z;
+          initialMeasurement = false;
+      }
+      float retain = (0.98);
+      gyroBias.z = g.z * (1- retain) + gyroBias.z * retain;
       /**
        * TODO: add a low-pass filter to update the bias
        */
@@ -169,6 +175,7 @@ public:
     // float estimatedPitchAngle = 0;
     // vector<float> eulerAngles;
     vector<float> gyroBias;
+    bool initialMeasurement = true;
 
     /* We make Robot a friend to avoid all the setters and getters. */
     friend class Robot;
diff --git a/src/robot.cpp b/src/robot.cpp
index 5f23335..3c644d0 100644
--- a/src/robot.cpp
+++ b/src/robot.cpp
@@ -1,5 +1,6 @@
 #include "robot.h"
 #include <IRdecoder.h>
+#include <limits.h>
 
 void Robot::InitializeRobot(void)
 {
@@ -79,11 +80,22 @@ void Robot::HandleOrientationUpdate(void)
 
     else // update orientation
     {
-        // TODO: update the orientation
+        float z_rot = ((float)imu.g.z - imu.gyroBias.z) / (float) INT_MAX;
+        z_rot /= 104;
+        z_rot *= 245;
+        eulerAngles.z += z_rot;
     }
 
 #ifdef __IMU_DEBUG__
-    Serial.println(eulerAngles.z);
+    Serial.print(imu.gyroBias.z);
+    Serial.print('\t');
+    Serial.print(eulerAngles.z);
+    Serial.print('\t');
+    //Serial.print(imu.g.x);
+    //Serial.print('\t');
+    //Serial.print(imu.g.y);
+    //Serial.print('\t');
+    Serial.println(imu.g.z);
 #endif
 }