Hunter tracking a bit better. Made some fixes to error calculation. Added a function to standarize angles to [0, 2pi)