In accordance with some embodiments of the present disclosure, systems and methods for a feedback based toolface control system for a rotary steerable drilling tool is disclosed. The method includes determining a desired toolface of a drilling tool, calculating a toolface error, calculating a correction to correct the toolface error by: estimating, using a model, an output of each of a plurality of states of the model based on an input to each of the states of the model and determining a desired input to each of the plurality of states, beginning at the toolface, based on a desired output of at least one of the plurality of states connected to a particular state, transmitting the signal to the input component of the drilling tool such that the signal adjusts the current toolface based on the correction, and drilling a wellbore with a drill bit oriented at the desired toolface.