Extending the detection algorithm, and kalman filter to 3D