One issue with this is the accelrometer. it is very very noisy for the non-gravity components of the vector it gives you. Also, if anything gets between your ultrasonics and the walls, it will mess up your numbers. I would recommend tracking the targets via the camera, it will be a lot less error prone and most likely easier to get working
