diff --git a/src/motionModel.cc b/src/motionModel.cc
index 1edba29..74d3c02 100644
--- a/src/motionModel.cc
+++ b/src/motionModel.cc
@@ -29,18 +29,15 @@ state_t MotionModel::update(vector<double> u_t0, vector<double> u_t1, state_t x_
     del_rot1 = atan2(u_t1[1] - u_t0[1], u_t1[0] - u_t0[0]) - u_t0[2];
     del_trans = sqrt(pow((u_t0[0] - u_t1[0]), 2) + pow((u_t0[1] - u_t1[1]), 2));
     del_rot2 = u_t1[2] - u_t0[2] - del_rot1;
-    
-//    del_rot1 = del_rot1 < 0 ? 2 * M_PI + del_rot1 : del_rot1;
-//    del_rot2 = del_rot2 < 0 ? 2 * M_PI + del_rot2 : del_rot2;
 
     // Standard deviation for each parmeter used in the next segment
-    term1 = alpha1 * del_rot1 + alpha2 * del_trans;
-    term2 = alpha3 * del_trans + alpha4 * (del_rot1 + del_rot2);
-    term3 = alpha1 * del_rot2 + alpha2 * del_trans;
+    term1 = alpha1 * abs(del_rot1) + alpha2 * abs(del_trans);
+    term2 = alpha3 * abs(del_trans) + alpha4 * abs(del_rot1 + del_rot2);
+    term3 = alpha1 * abs(del_rot2) + alpha2 * abs(del_trans);
 
-    normal_distribution<double> d1{0, sqrt(abs(term1))};
-    normal_distribution<double> d2{0, sqrt(abs(term2))};
-    normal_distribution<double> d3{0, sqrt(abs(term3))};
+    normal_distribution<double> d1{0, sqrt(term1)};
+    normal_distribution<double> d2{0, sqrt(term2)};
+    normal_distribution<double> d3{0, sqrt(term3)};
 
     // Gaussian noise distribution with zero mean and sd for each paramter,
     // thereby modelling the noise of each parameter