Stochastic Real-Time Optimal Control for Bearing-Only Trajectory Planning

A method is presented to simultaneously solve the optimal control problem and the optimal estimation problem for a bearing-only sensor. For bearing-only systems that require a minimum level of certainty in position relative to a source for mission accomplishment, some amount of maneuver is required to measure range. Traditional methods of trajectory optimization and optimal estimation minimize an information metric. This paper proposes constraining the final value of the information states with known time propagation dynamics relative to a given trajectory which allows for attainment of the required level of information with minimal deviation from a general performance index that can be tailored to a specific vehicle. The proposed method does not suffer from compression of the information metric into a scalar, and provides a route that will attain a particular target estimate quality while maneuvering to a desired relative point or set. An algorithm is created to apply the method in real-time, iteratively estimating target position with an Unscented Kalman Filter and updating the trajectory with an efficient pseudospectral method. Methods and tools required for hardware implementation are presented that apply to any real-time optimal control (RTOC) system. The algorithm is validated with both simulation and flight test, autonomously landing a quadrotor on a wire.