Potentially better motion model

[satrajit-chatterjee]

Problem

Currently we are using a unicycle motion model with proportionality gains that controls heading and vertical velocity.

However, this could potential issues.

Firstly, for our horizontal motion dynamics, we may fail to enter target waypoints at desired headings if they are too far apart.

[bradygm]

Could swap out for dubins following using my previous sim.