diff --git a/src/create.cpp b/src/create.cpp
index e77a560..b0cc473 100644
--- a/src/create.cpp
+++ b/src/create.cpp
@@ -66,10 +66,10 @@ namespace create {
     float dt = (curTime - prevOnDataTime) / 1000000.0;
     float deltaDist, deltaX, deltaY, deltaYaw;
     if (model == CREATE_1) {
-      deltaDist = GET_DATA(ID_DISTANCE) / 1000.0; //mm -> m
+      deltaDist = ((int16_t) GET_DATA(ID_DISTANCE)) / 1000.0; //mm -> m
       deltaYaw = ((int16_t) GET_DATA(ID_ANGLE)) * (util::PI / 180.0); // D2R
-      deltaX = deltaDist * cos(pose.yaw);
-      deltaY = deltaDist * sin(pose.yaw);
+      deltaX = deltaDist * cos( util::normalizeAngle(pose.yaw + deltaYaw) );
+      deltaY = deltaDist * sin( util::normalizeAngle(pose.yaw + deltaYaw) );
     }
     else if (model == CREATE_2) {
       // Get cumulative ticks (wraps around at 65535)