Moved yaw correction to its proper location (between velocity PID output and before pitch/roll PID input).
Eliminated leftover graph_add_node_SpecificTypeHere() functions.

Resets recursively, keeping track of children. Created tests, which pass. Computation terminates on graph cycles. Added comments.