Dead Reckoning

To Run

Run rosrun dead_reckoning dead_reckoning.py

Subscribed Topics

/encoder1_raw, Int64

/encoder2_raw, Int64

/BNO055, Imu

Published Topics

/odom, Odometry

/tf, TransformStamped

Details

This node takes in the wheel velocity and orientation from IMU to calculate a state estimate with the following dead reckoning algorithm, which takes a forward displacement vector and rotates it by the current IMU orientation:

x = 0
y = 0
z = 0
roll = 0
pitch = 0
yaw = 0

r_prev = 0
l_prev = 0

loop:
   # retrieve information from encoders and BNO055 IMU
   (roll, pitch, yaw) <- /BNO055
   r_cur <- /encoder1_raw
   l_cur <- /encoder2_raw

   # determine how far forward encoders state buggy has traveled
   avg_diff = ((r_cur - r_prev) + (l_cur - l_prev))/2.0
   f_dist = avg_diff * TICKS_TO_M

   # create rotation matrices to transform forward displacement
   yawMatrix = np.matrix([
               [math.cos(yaw), -math.sin(yaw), 0],
               [math.sin(yaw), math.cos(yaw), 0],
               [0, 0, 1]
               ])

   pitchMatrix = np.matrix([
               [math.cos(pitch), 0, math.sin(pitch)],
               [0, 1, 0],
               [-math.sin(pitch), 0, math.cos(pitch)]
               ])

   rollMatrix = np.matrix([
               [1, 0, 0],
               [0, math.cos(roll), -math.sin(roll)],
               [0, math.sin(roll), math.cos(roll)]
               ])

   R = yawMatrix * pitchMatrix * rollMatrix
   disp_mat = np.matrix([f_dist], [0], [0](/AtlasBuggy/BabyBuggyROS/wiki/f_dist],-[0],-[0))

   # multiply forward displacement vector by rotation matrix to get 3D displacement
   transf_vel = R * disp_mat

   # update position state variables
   x += transf_vel[0]
   y += transf_vel[1]
   z += transf_vel[2]

Note that the algorithm above uses a three rotation matrices to rotate f_dist to update the position in x, y, and z dimensions.