diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp
index 29b30e36..c3b80a0e 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -1,5 +1,8 @@
 #include "components/motion/MotionController.h"
-#include "os/os_cputime.h"
+
+#include <FreeRTOS.h>
+#include <task.h>
+
 using namespace Pinetime::Controllers;
 
 void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps) {
@@ -12,8 +15,11 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps)
   }
 
   this->x = x;
+  lastY = this->y;
   this->y = y;
+  lastZ = this->z;
   this->z = z;
+
   int32_t deltaSteps = nbSteps - this->nbSteps;
   this->nbSteps = nbSteps;
   if (deltaSteps > 0) {
@@ -48,7 +54,7 @@ bool MotionController::ShouldShakeWake(uint16_t thresh) {
   auto diff = xTaskGetTickCount() - lastShakeTime;
   lastShakeTime = xTaskGetTickCount();
   /* Currently Polling at 10hz, If this ever goes faster scalar and EMA might need adjusting */
-  int32_t speed = std::abs(z + (y / 2) + (x / 4) - lastYForShake - lastZForShake) / diff * 100;
+  int32_t speed = std::abs(z + (y / 2) + (x / 4) - lastY / 2 - lastZ) / diff * 100;
   //(.2 * speed) + ((1 - .2) * accumulatedSpeed);
   // implemented without floats as .25Alpha
   accumulatedSpeed = (speed / 5) + ((accumulatedSpeed / 5) * 4);
@@ -56,9 +62,6 @@ bool MotionController::ShouldShakeWake(uint16_t thresh) {
   if (accumulatedSpeed > thresh) {
     wake = true;
   }
-  lastXForShake = x / 4;
-  lastYForShake = y / 2;
-  lastZForShake = z;
   return wake;
 }
 
diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h
index 9006b2a6..8fb03d81 100644
--- a/src/components/motion/MotionController.h
+++ b/src/components/motion/MotionController.h
@@ -1,8 +1,9 @@
 #pragma once
 
 #include <cstdint>
-#include <drivers/Bma421.h>
-#include <components/ble/MotionService.h>
+
+#include "drivers/Bma421.h"
+#include "components/ble/MotionService.h"
 
 namespace Pinetime {
   namespace Controllers {
@@ -63,17 +64,18 @@ namespace Pinetime {
     private:
       uint32_t nbSteps;
       uint32_t currentTripSteps = 0;
+
       int16_t x;
-      int16_t y;
-      int16_t z;
       int16_t lastYForWakeUp = 0;
+      int16_t lastY = 0;
+      int16_t y;
+      int16_t lastZ = 0;
+      int16_t z;
+
       bool isSensorOk = false;
       DeviceTypes deviceType = DeviceTypes::Unknown;
       Pinetime::Controllers::MotionService* service = nullptr;
 
-      int16_t lastXForShake = 0;
-      int16_t lastYForShake = 0;
-      int16_t lastZForShake = 0;
       int32_t accumulatedSpeed = 0;
       uint32_t lastShakeTime = 0;
     };