Updated sonar model, fixed wall detection