I learnt the equations of motions from https://arxiv.org/pdf/2302.08100.pdf. The mass matrix is for their ASV, and I assume T for frame transformation is identity matrix.
This is a test where there is not environment interference. I set input surge velocity to 1, input sway velocity to 0, and input yaw rate to 0.1. The output graph trajectory_for_no_environment_force looks good.
Add more demos to prove that the model is working