  Most mobile robots follow a modular sense-planact system architecture that
can lead to poor performance or even catastrophic failure for visual inertial
navigation systems due to trajectories devoid of feature matches. Planning in
belief space provides a unified approach to tightly couple the perception,
planning and control modules, leading to trajectories that are robust to noisy
measurements and disturbances. However, existing methods handle uncertainties
as costs that require manual tuning for varying environments and hardware. We
therefore propose a novel trajectory optimization formulation that incorporates
inequality constraints on uncertainty and a novel Augmented Lagrangian based
stochastic differential dynamic programming method in belief space.
Furthermore, we develop a probabilistic visibility model that accounts for
discontinuities due to feature visibility limits. Our simulation tests
demonstrate that our method can handle inequality constraints in different
environments, for holonomic and nonholonomic motion models with no manual
tuning of uncertainty costs involved. We also show the improved optimization
performance in belief space due to our visibility model.
