Remove debugging code
Better calculation of north vec