I had to compensate the bearing being feed into the method. It was exspecting a float 0-360 and was receiving a float -180 to 180. This simple fix increased the robot's navivation accuracy Change-Id: I9f29d8c36a2d2e840bb3ee68303d2c77f6c0e5be Signed-off-by: Ricky Barrette <rickbarrette@gmail.com>