Modified motion model equations by including abs values

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