pprz_algebra_float.h new functions (#2156)

* added inversion, identity, and  add_scalar functions to pprz_algebra_float.h
* changed to double arrays + verified
* moved invert function from inline to pprz_algebra_float.c file

sw/airborne/math/pprz_algebra_float.c

@@ -765,3 +765,54 @@ bool float_mat_inv_4d(float invOut[16], float mat_in[16])
 
   return 0; //success
 }
+
+/** Calculate inverse of any n x n matrix (passed as C array) o = mat^-1
+Algorithm verified with Matlab. 
+Thanks to: https://www.quora.com/How-do-I-make-a-C++-program-to-get-the-inverse-of-a-matrix-100-X-100
+*/
+void float_mat_invert(float **o, float **mat, int n)
+{
+  int i, j, k;
+  float t;
+  float a[n][2*n];
+
+  // Append an identity matrix on the right of the original matrix
+  for(i = 0; i < n; i++) {
+    for(j = 0; j < 2*n; j++) {
+      if (j < n) {
+        a[i][j] = mat[i][j];
+      }
+      else if( (j >= n) && (j == i+n)) {
+        a[i][j] = 1.0;
+      }
+      else {
+        a[i][j] = 0.0;
+      }
+    } 
+  }
+
+  // Do the inversion
+  for( i = 0; i < n; i++) {
+    t = a[i][i];     // Store diagonal variable (temp)
+
+    for(j = i; j < 2*n; j++) {
+      a[i][j] = a[i][j]/t; // Divide by the diagonal value
+    }
+
+    for(j = 0; j < n; j++) {
+      if( i!=j ) {
+        t = a[j][i];
+        for(k=0; k<2*n; k++) {
+          a[j][k] = a[j][k] - t*a[i][k];
+        }
+      }
+    }
+  }
+
+  // Cut out the identity, which has now moved to the left side
+  for(i = 0 ; i < n ; i++ ) {
+    for(j = n; j < 2*n; j++ ) {
+      o[i][j-n] = a[i][j];
+    }
+  }
+}

sw/airborne/math/pprz_algebra_float.h

@@ -620,6 +620,8 @@ static inline float float_vect_dot_product(const float *a, const float *b, const
     for (__i = 0; __i < _rows; __i++) { _ptr[__i] = &_mat[__i][0]; } \
   }
 
+extern void float_mat_invert(float **o, float **mat, int n);
+
 /** a = 0 */
 static inline void float_mat_zero(float **a, int m, int n)
 {
@@ -729,6 +731,22 @@ static inline void float_mat_col(float *o, float **a, int m, int c)
   }
 }
 
+/** Make an n x n identity matrix (for matrix passed as array) */
+static inline void float_mat_identity(float **o, int n)
+{
+  int i, j;
+  for(i = 0 ; i < n; i++) {
+    for(j = 0 ; j < n; j++) {
+      if (i == j) {
+        o[i][j] = 1.0;
+      }
+      else {
+        o[i][j] = 0.0;
+      }
+    }
+  }
+};
+
 extern bool float_mat_inv_2d(float inv_out[4], float mat_in[4]);
 extern void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in);
 extern bool float_mat_inv_4d(float invOut[16], float mat_in[16]);