T-LQG: Closed-loop belief space planning via trajectory-optimized LQG

Planning under motion and observation uncertainties requires the solution of a stochastic control problem in the space of feedback policies. In this paper, by restricting the policy class to the linear feedback polices, we reduce the general (n2 + n)-dimensional belief space planning problem to an (n)-dimensional problem. As opposed to the previous literature that search in the space of open-loop optimal control policies, we obtain this reduction in the space of closed-loop policies by obtaining a Linear Quadratic Gaussian (LQG) design with the best nominal performance. Then, by taking the entire underlying trajectory of the LQG controller as the decision variable, we pose a coupled design of the trajectory and estimator (while keeping the design of the controller separate) as a NonLinear Program (NLP) that can be solved by a general NLP solver. We prove that under a first-order approximation and a careful usage of the separation principle, our approximations are valid. We provide an analysis on the existing major belief space planning methods and show that our algorithm keeps the lowest computational burden while searching in the policy space. Finally, we extend our solution to contain general state and control constraints. Our simulation results support our design.