Issue was in the plotting. Specifically the conversion of the angular states from radians to degrees.
The conversion was unintentionally set to include the Z position states (as opposed to the angular states only).
It also explains why setting the initial Z position as non-zero yielded unexpected results.
In summary, the correction addresses both of the issues @pedropereira97 brought up.
Issue was in the plotting. Specifically the conversion of the angular states from radians to degrees. The conversion was unintentionally set to include the Z position states (as opposed to the angular states only). It also explains why setting the initial Z position as non-zero yielded unexpected results.
In summary, the correction addresses both of the issues @pedropereira97 brought up.