This week I have been debugging blocks other than the attitude controller. After finding a transformation from the body frame to the control frame, new matrices from linearization is generated. However, because the rotation matrix from the original paper is unknown, it is not possible to directly compare the resultant state-space form and gain.
I also made adjustment to the thrust block,replacing the current body angular velocity input with the constant angular velocity from the hover solution. A problem with the current design is that the position thrust block only knows the magnitude of the desired acceleration but does not know the direction. Therefore, once the drone overshoots, the controller has no way to return to the desired location.
