Fixed orientation bug, calculate sonar peaks with global orientation