diff --git a/esp32/include/motion.h b/esp32/include/motion.h
index a3114b0..0f5f8b0 100644
--- a/esp32/include/motion.h
+++ b/esp32/include/motion.h
@@ -107,6 +107,7 @@ class MotionService {
 
     bool updateMotion() {
         if (!state) return false;
+        state->updateImuOffsets(_peripherals->angleY(), _peripherals->angleX());
         state->step(body_state);
         kinematics.calculate_inverse_kinematics(body_state, new_angles);
 
diff --git a/esp32/include/motion_states/stand_state.h b/esp32/include/motion_states/stand_state.h
index b9eb8ba..6add493 100644
--- a/esp32/include/motion_states/stand_state.h
+++ b/esp32/include/motion_states/stand_state.h
@@ -26,7 +26,7 @@ class StandState : public MotionState {
     }
 
     void step(body_state_t &body_state, float dt = 0.02f) override {
-        lerpToBody(body_state);
+        lerpToBody(body_state, true);
         updateFeet(body_state);
     }
 };
\ No newline at end of file
diff --git a/esp32/include/motion_states/state.h b/esp32/include/motion_states/state.h
index 02c22a1..3a9f500 100644
--- a/esp32/include/motion_states/state.h
+++ b/esp32/include/motion_states/state.h
@@ -9,13 +9,17 @@ class MotionState {
     static constexpr const float (&default_feet_pos)[4][4] = KinConfig::default_feet_positions;
     body_state_t target_body_state;
     static constexpr float default_smoothing_factor = 0.06f;
+    float omega_offset = 0, psi_offset = 0;
 
-    void lerpToBody(body_state_t& body_state, const float smoothing_factor = default_smoothing_factor) {
+    void lerpToBody(body_state_t& body_state, const bool imuCompensate = false,
+                    const float smoothing_factor = default_smoothing_factor) {
         body_state.xm = lerp(body_state.xm, target_body_state.xm, smoothing_factor);
         body_state.ym = lerp(body_state.ym, target_body_state.ym, smoothing_factor);
         body_state.zm = lerp(body_state.zm, target_body_state.zm, smoothing_factor);
         body_state.phi = lerp(body_state.phi, target_body_state.phi, smoothing_factor);
-        body_state.psi = lerp(body_state.psi, target_body_state.psi, smoothing_factor);
+        body_state.psi = lerp(body_state.psi, target_body_state.psi - imuCompensate * psi_offset, smoothing_factor);
+        body_state.omega =
+            lerp(body_state.omega, target_body_state.omega - imuCompensate * omega_offset, smoothing_factor);
     }
 
     void updateFeet(body_state_t& body_state, const float smoothing_factor = default_smoothing_factor) {
@@ -25,6 +29,10 @@ class MotionState {
     }
 
   public:
+    void updateImuOffsets(const float omega_offset, const float psi_offset) {
+        this->omega_offset = omega_offset * RAD2DEG_F;
+        this->psi_offset = psi_offset * RAD2DEG_F;
+    }
     virtual ~MotionState() {}
 
     virtual void begin() { ESP_LOGI("Gait Planner", "Starting %s", name()); }
diff --git a/esp32/include/motion_states/walk_state.h b/esp32/include/motion_states/walk_state.h
index 8a19100..1bcdf44 100644
--- a/esp32/include/motion_states/walk_state.h
+++ b/esp32/include/motion_states/walk_state.h
@@ -23,7 +23,7 @@ class WalkState : public MotionState {
     float phase_offset[4] = {0.f, 0.5f, 0.5f, 0.f};
     float stand_offset = 0.6f;
     float step_length = 0.0f;
-    float speed_factor = 1;
+    float speed_factor = 2;
     gait_state_t gait_state;
     gait_state_t target_gait_state;