Fixed some bugs in the sonar filter