In this paper, we have addressed a novel approach to compute the probability of collision under robot and obstacle pose uncertainties. The collision probability is computed as an infinite series whose convergence is proved. An upper bound for the truncation error is also derived. As shown in Fig. REF , convergence analysis is performed for different configurations and it is seen that our approach is of the order of milliseconds and therefore can be used in online planning. We also provide a comparison with the approaches in [1]}, [2]}. In addition, we incorporate landmark uncertainties in belief space planning and derive the resulting Bayes filter in terms of the prediction and measurement updates of the EKF. Finally, experimental evaluation for a mobile robot scenario and a 2D manipulator is performed to illustrate our approach. We have considered static obstacles in this paper and the immediate future work is to realize the approach in simulated and real-world environments with dynamic obstacles.
