Path planning to optimize observability in a planar uniform flow field

This paper is concerned with path planning for under-sensed vehicles, where the vehicle has insufficient sensors to estimate its state without active control. One such system is an autonomous underwater vehicle, which typically does not have complete inertial position information. Using the condition number of the observability gramian as a cost functional, we develop an observability optimal control problem for a general nonlinear system and discuss solution methods. Two planar navigation problems for a nonholonomic vehicle in uniform currents are presented, and we derive trajectories that maximize the observability of the system and currents using limited inertial sensors. Simulation results show that the optimal trajectories yield faster estimator convergence and lower steady-state covariances than suboptimal trajectories.