Performed testing on the program. It ultimately will converge on the correct location but the loop time needs to be sped up by a factor of 3-6x