Optimal Active Sensing Control for Two-Frame Systems

This paper provides a complete characterization of the trajectories that maximize the information collected by a moving vehicle, through sensors' measurements, for the recently introduced class of nonlinear “two-frame systems”. The information is quantified in terms of the trace of the observability Gramian (OG) along a trajectory. In general, this quantity nontrivially depends on the control inputs and the state trajectory, resulting in a difficult optimal control problem. Herein, we leverage the property of invariant filtering that Jacobians are state-trajectory independent, that is, only depend on the control inputs, which enables us to mathematically derive optimal trajectories in closed form. We illustrate the results numerically on problems from robotics such as 3D robot localization, and 2D simultaneous localization and mapping.