Integrated Navigation & Path Planning in a Discontinuous Measurement Update Field

This work presents an optimal control framework for path planning in a spatially and temporally discontinuous measurement field, specifically applied to an unmanned extraterrestrial vehicle on Mars. The framework incorporates state uncertainty quantification within an optimal control problem (OCP), allowing adaptability to diverse mission requirements. An example scenario is explored, involving a UAV mission on Mars with sparse Mars Digital Elevation Maps (DEM), inspired by NASA's Ingenuity vehicle [1]. The proposed framework formulates an OCP subjected to dynamic constraints, path constraints, endpoint constraints, endpoint cost, and running cost. Notably, it addresses the challenge of state uncertainty quantification integrated within the OCP formulation. Linear and non-linear Kalman filters are employed for state uncertainty quantification, leveraging a covariance propagation model. Additionally, functional models are used to account for the discontinuous measurement field by scaling the measurement noise covariance matrix. Moreover, this work highlights the significance of the initial control guess in solving the OCP. A novel search method is derived to provide an accurate and efficient initial control guess, enhancing the convergence and effectiveness of the optimization process.