Decently working state with improvements

[william-swy]

First thing to note: massive changes from baseline commit of 3248add

What changed:

• Separated functions out. Makes the huge derivative calculator much easier on the eyes.
• No more gimbal lock with angles close to 90 degrees
• More flexible center of mass calculations. No more of what it appeared to be a polynomial spline? Moment of inertia calculations could be further improved
• Some considerations of thrust misalignment but not perfect
• Improved wind model. No more constant wind speed. Although it seems to significantly increase the runtime.
• More flexible drag model. Takes in vehicle parameters.
• Sampling is done in parallel. Improves runtime.
• Some unit tests. There is more certainty that there is no bug with the math.

Might be a good idea to keep the baseline code in a separate branch as there is some good stuff in there.