Fixed some equations for mock GPS. Need to check accurary and add scaling