Abstract: A method for controlling a drilling trajectory of a downhole tool includes receiving a drilling plan for a downhole tool to drill a wellbore toward a target in a subterranean formation. The method also includes receiving a measured drilling trajectory of the downhole tool after the downhole tool drills a first portion of the wellbore using the planned drilling trajectory. The method also includes determining a state of the downhole tool based at least partially upon the planned drilling trajectory and the measured drilling trajectory. The state includes a difference between the planned drilling trajectory and the measured drilling trajectory, a level of control of a steering capability of the downhole tool, and a location of an end of the wellbore. The method also includes generating a working plan trajectory based at least partially upon the state of the downhole tool and the drilling plan.