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.
INTRODUCTIONBearing-only tracking is a classical navigation problem. Many real-world systems depend on angleonly sensors for target state estimation, such as submarines using only passive sonar, high-speed antiradiation missiles (HARM), robots, and unmanned aerial vehicles (UAVs) using images from an optical sensor. The inability to sense range with each measurement, combined with the inherent nonlinearity of the problem make estimation of a target's location and motion problematic. Moving the sensor orthogonal to the line-of-sight (LOS) generates observability for range estimation, and many researchers have pursued optimal methods for trajectory planning in this context [1][2][3][4][5][6]. Optimal trajectory planning of any sort, however, is notoriously difficult to implement for on-line systems (excepting those simple enough to have a closed-form analytical feedback solution). Notably, recent advances in direct optimization through techniques such as pseudospectral methods (PSM) [7][8][9] have increased the efficiency and stability of obtaining a numeric optimal solution to the point where realtime optimal control (RTOC) of systems with moderate speed dynamics is now possible. This paper proposes a new method of approaching the bearing-only trajectory planning problem that enables simultaneous consideration of both the optimal control problem and a system's prescribed final estimation requirements, overcoming the typical limitations of previous approaches which address each requirement separately. The trajectory planning goal is to provide an optimal trajectory for arrival at a point, or set of points, potentially offset from a relative targe...