Closed-loop belief space planning for linear, Gaussian systems

This paper considers the problem of motion planning for linear, Gaussian systems, and extends existing chance constrained optimal control solutions [1], [2] by incorporating the closed-loop uncertainty of the system and by reducing the conservativeness in the constraints. Due to the imperfect knowledge of the system state caused by motion uncertainty and sensor noise, the constraints cannot be guaranteed to be satisfied and consequently must be considered probabilistically. In this work, they are formulated as convex constraints on a univariate Gaussian random variable, with the violation probability of all the constraints guaranteed to be below a threshold. This threshold is a tuning parameter which trades off the performance of the system and the conservativeness of the solution. In contrast to similar methods, the proposed work considers the specific estimator and controller used in the closed-loop system in order to directly characterize the a priori distribution of the closed-loop system state. Using this distribution, a convex optimization program is formulated to solve for the optimal solution for the closed-loop system. The performance of the algorithm is demonstrated through several examples.