# Dead Reckoning Node - AtlasBuggy/BabyBuggyROS Wiki

### 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], , ])

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

# update position state variables
x += transf_vel
y += transf_vel
z += transf_vel
``````

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