Added Cmat in hydrodynamic forces equation

[rmvanarse]

This PR modifies the hydrodynamic forces equation in usv_gazebo_dynamics_plugin.cc to include the Coriolis centripetal matrix for added mass, Cmat.

As mentioned in #441, the model used in the code does not match the Theory of operation (see eq. 1).

Note: There also seem to be differences in the terms present in Cmat in Theory of operation, this paper and this definition. These differences have not been addressed in this PR.

Need to verify if switching to this model is causing any unexpected behaviour in the USV. Do you remember if Cmat was purposely omitted, and if so, the reason for doing so?

[acxz]

Adding the Coriolis hydro forces will be great to increase the fidelity of VRX. If users do not want to simulate these forces, the Coriolis params can always be set to zero.

[caguero]

@rmvanarse , we got some time to help with your contribution. Are you willing to retarget it to the jazzy branch?

[rmvanarse]

Hi @caguero , would you like me to rebase over jazzy? I can also re-open a separate PR with the same changes if that's easier and there aren't any other changes.
Is there a jazzy equivalent of the file I had changed?

[caguero]

Hi @caguero , would you like me to rebase over jazzy? I can also re-open a separate PR with the same changes if that's easier and there aren't any other changes. Is there a jazzy equivalent of the file I had changed?

Whatever it's easier for you works for us :)

I think the hydrodynamics plugin should be pretty much the same.

[caguero]

See #829