Fixed tdoa algorithm and added changes to other functions for functionality