Added odometry-based motion model function

src/cs4313_utilities/robot_math.py

@@ -129,6 +129,35 @@ def normalize_pi(angle):
 
 # Sensor and motion model functions
 
+def odometryMotionModel(state, odom1, odom2, alpha1, alpha2, alpha3, alpha4):
+    ''' Implements the CS4313 odometry-based motion model (lecture 3.2)
+    @param state: hypothetical state of the form ( x, y, theta )
+    @param odom1: odometry value at time t-1
+    @param odom2: odometry value at time t
+    @param alpha1: motion model angular noise turn component parameter
+    @param alpha2: motion model angular noise translation component parameter
+    @param alpha3: motion model linear noise translation component parameter
+    @param alpha4: motion model linear noise turn component parameter
+    @return a tuple ( x, y, theta ) of the motion model application
+    '''
+    # Compute noise-free motion
+    ( odom_x, odom_y ) = ( odom2[0] - odom1[0], odom2[1] - odom1[1] )
+    d_trans = math.hypot(odom_x, odom_y)
+    d_rot1 = normalize_pi(math.atan2(odom_y, odom_x) - odom1[2])
+    d_rot2 = normalize_pi(odom2[2] - odom1[2] - d_rot1)
+
+    # Add noise to compute "true" motion
+    d_trans += random.gauss(0.0, alpha3 * abs(d_trans) + alpha4 * abs(d_rot1 + d_rot2))
+    d_rot1 += random.gauss(0.0, alpha1 * abs(d_rot1) + alpha2 * abs(d_trans))
+    d_rot2 += random.gauss(0.0, alpha1 * abs(d_rot2) + alpha2 * abs(d_trans))
+
+    # Apply the computed motion to the original state
+    x_t = state[0] + d_trans * math.cos(odom1[2] + d_rot1)
+    y_t = state[1] + d_trans * math.sin(odom1[2] + d_rot1)
+    theta_t = normalize(state[1] + d_rot1 + d_rot2)
+    return ( x_t, y_t, theta_t )
+
+
 def likelihoodFieldModel(z, z_max, distances, sigma, \
                          alpha_hit, alpha_max, alpha_rand):
     ''' Implements the likelihood field algorithm discussed in class to