Unifying math functions

[knmcguire]

In both the controllers and estimators, math functions are being rewritten as part of the algorithm, but it would be better to separate those all in a unified math .c files or such. We already have math3d.h and such, but some of the latest contributions has not been as consistent.

Here is a list of those functions that qualify, so more can be added in the future:

• crazyflie-firmware/src/modules/src/kalman_core/mm_tdoa_robust.c

  Lines 25 to 27 in af96c07

  	// Cholesky Decomposition for a nxn psd matrix (from scratch)
  	// Reference: https://www.geeksforgeeks.org/cholesky-decomposition-matrix-decomposition/
  	static void Cholesky_Decomposition(int n, float matrix[n][n], float lower[n][n]){

[whoenig]

This should also include the data structures. For example, there are multiple different definitions of 3d vectors and quaternions in the code.

[knmcguire]

good point! I changed the title.

[krichardsson]

Also include definitions of pi (I found 3) :-)