The following steps are used to update the dynamics of an aerial robot.
- Calculate the rotation matrix R from the last iteration's Euler Angle θ.
- Calculate angular acceleration given current force and torque inputs, and the angular velocity from last iteraion.
- Update angular velocity
- Calculate Euler angle rate with old Euler angle and new angular velocity
- Update Euler angle
- Calculate acceleration and integrate to update position
I wrote a new script to test the implementation. The Euler angle with 0.001 N of constant thrust from propeller is shown here: