Add object cause altitude control problem

by adding any object from Gazebo objects or Sphinx objects like a wall, when the robot comes near the object will get in trouble in altitude controller and increase its height. I think it’s because of the wide field of view for the ultrasonic sensor used in the simulator.