Future Uncertainty-Based Control for Relative Navigation in GPS-Denied Environments

In a nonlinear system where the separation principle does not hold, control actions at the current time can have a significant effect on estimation accuracy in the future. To properly control this type of system with a model predictive controller requires an accurate estimate of the future uncertainty of the system. To compute this estimate, we propose a definition of the nonlinear observability matrix with respect to a future state and establish its connection with the filtering Cramer-Rao lower bound at the future state. We also define a sensitivity function that correlates noise on control inputs to errors on the state estimate. As a demonstrative example, we consider the problem of relative navigation of two unmanned aerial systems in Global Positioning System denied environments. We demonstrate using Monte Carlo simulations that by controlling to optimize our derived observability and controllability metrics, we achieve significantly improved estimation performance over a trajectory optimized for an uncertainty estimate that does not consider input noise.