diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
index 8b94f1fbba..0887057ed2 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -828,7 +828,7 @@ void calc_g1g2_pseudo_inv(void)
   float_vect_scale(g1g2_trans_mult[0], 1000.0, INDI_OUTPUTS * INDI_OUTPUTS);
 
   //inverse of 4x4 matrix
-  float_mat_inv_4d(g1g2inv[0], g1g2_trans_mult[0]);
+  float_mat_inv_4d(g1g2inv, g1g2_trans_mult);
 
   //scale back
   float_vect_scale(g1g2inv[0], 1000.0, INDI_OUTPUTS * INDI_OUTPUTS);
diff --git a/sw/airborne/math/pprz_algebra_float.c b/sw/airborne/math/pprz_algebra_float.c
index d170d530dc..9352b95204 100644
--- a/sw/airborne/math/pprz_algebra_float.c
+++ b/sw/airborne/math/pprz_algebra_float.c
@@ -818,40 +818,40 @@ void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect
  * obtained from: http://rodolphe-vaillant.fr/?e=7
  */
 
-static float float_mat_minor_4d(float m[16], int r0, int r1, int r2, int c0, int c1, int c2)
+static float float_mat_minor_4d(float m[4][4], int r0, int r1, int r2, int c0, int c1, int c2)
 {
-  return m[4 * r0 + c0] * (m[4 * r1 + c1] * m[4 * r2 + c2] - m[4 * r2 + c1] * m[4 * r1 + c2]) -
-         m[4 * r0 + c1] * (m[4 * r1 + c0] * m[4 * r2 + c2] - m[4 * r2 + c0] * m[4 * r1 + c2]) +
-         m[4 * r0 + c2] * (m[4 * r1 + c0] * m[4 * r2 + c1] - m[4 * r2 + c0] * m[4 * r1 + c1]);
+  return m[r0][c0] * (m[r1][c1] * m[r2][c2] - m[r2][c1] * m[r1][c2]) -
+         m[r0][c1] * (m[r1][c0] * m[r2][c2] - m[r2][c0] * m[r1][c2]) +
+         m[r0][c2] * (m[r1][c0] * m[r2][c1] - m[r2][c0] * m[r1][c1]);
 }
 
 
-static void float_mat_adjoint_4d(float adjOut[16], float m[16])
+static void float_mat_adjoint_4d(float adjOut[4][4], float m[4][4])
 {
-  adjOut[ 0] =  float_mat_minor_4d(m, 1, 2, 3, 1, 2, 3);
-  adjOut[ 1] = -float_mat_minor_4d(m, 0, 2, 3, 1, 2, 3);
-  adjOut[ 2] =  float_mat_minor_4d(m, 0, 1, 3, 1, 2, 3);
-  adjOut[ 3] = -float_mat_minor_4d(m, 0, 1, 2, 1, 2, 3);
-  adjOut[ 4] = -float_mat_minor_4d(m, 1, 2, 3, 0, 2, 3);
-  adjOut[ 5] =  float_mat_minor_4d(m, 0, 2, 3, 0, 2, 3);
-  adjOut[ 6] = -float_mat_minor_4d(m, 0, 1, 3, 0, 2, 3);
-  adjOut[ 7] =  float_mat_minor_4d(m, 0, 1, 2, 0, 2, 3);
-  adjOut[ 8] =  float_mat_minor_4d(m, 1, 2, 3, 0, 1, 3);
-  adjOut[ 9] = -float_mat_minor_4d(m, 0, 2, 3, 0, 1, 3);
-  adjOut[10] =  float_mat_minor_4d(m, 0, 1, 3, 0, 1, 3);
-  adjOut[11] = -float_mat_minor_4d(m, 0, 1, 2, 0, 1, 3);
-  adjOut[12] = -float_mat_minor_4d(m, 1, 2, 3, 0, 1, 2);
-  adjOut[13] =  float_mat_minor_4d(m, 0, 2, 3, 0, 1, 2);
-  adjOut[14] = -float_mat_minor_4d(m, 0, 1, 3, 0, 1, 2);
-  adjOut[15] =  float_mat_minor_4d(m, 0, 1, 2, 0, 1, 2);
+  adjOut[0][0] =  float_mat_minor_4d(m, 1, 2, 3, 1, 2, 3);
+  adjOut[0][1] = -float_mat_minor_4d(m, 0, 2, 3, 1, 2, 3);
+  adjOut[0][2] =  float_mat_minor_4d(m, 0, 1, 3, 1, 2, 3);
+  adjOut[0][3] = -float_mat_minor_4d(m, 0, 1, 2, 1, 2, 3);
+  adjOut[1][0] = -float_mat_minor_4d(m, 1, 2, 3, 0, 2, 3);
+  adjOut[1][1] =  float_mat_minor_4d(m, 0, 2, 3, 0, 2, 3);
+  adjOut[1][2] = -float_mat_minor_4d(m, 0, 1, 3, 0, 2, 3);
+  adjOut[1][3] =  float_mat_minor_4d(m, 0, 1, 2, 0, 2, 3);
+  adjOut[2][0] =  float_mat_minor_4d(m, 1, 2, 3, 0, 1, 3);
+  adjOut[2][1] = -float_mat_minor_4d(m, 0, 2, 3, 0, 1, 3);
+  adjOut[2][2] =  float_mat_minor_4d(m, 0, 1, 3, 0, 1, 3);
+  adjOut[2][3] = -float_mat_minor_4d(m, 0, 1, 2, 0, 1, 3);
+  adjOut[3][0] = -float_mat_minor_4d(m, 1, 2, 3, 0, 1, 2);
+  adjOut[3][1] =  float_mat_minor_4d(m, 0, 2, 3, 0, 1, 2);
+  adjOut[3][2] = -float_mat_minor_4d(m, 0, 1, 3, 0, 1, 2);
+  adjOut[3][3] =  float_mat_minor_4d(m, 0, 1, 2, 0, 1, 2);
 }
 
-static float float_mat_det_4d(float m[16])
+static float float_mat_det_4d(float m[4][4])
 {
-  return m[0] * float_mat_minor_4d(m, 1, 2, 3, 1, 2, 3) -
-         m[1] * float_mat_minor_4d(m, 1, 2, 3, 0, 2, 3) +
-         m[2] * float_mat_minor_4d(m, 1, 2, 3, 0, 1, 3) -
-         m[3] * float_mat_minor_4d(m, 1, 2, 3, 0, 1, 2);
+  return m[0][0] * float_mat_minor_4d(m, 1, 2, 3, 1, 2, 3) -
+         m[0][1] * float_mat_minor_4d(m, 1, 2, 3, 0, 2, 3) +
+         m[0][2] * float_mat_minor_4d(m, 1, 2, 3, 0, 1, 3) -
+         m[0][3] * float_mat_minor_4d(m, 1, 2, 3, 0, 1, 2);
 }
 
 /**
@@ -860,7 +860,7 @@ static float float_mat_det_4d(float m[16])
  * @param invOut output array, inverse of mat_in
  * @param mat_in input array
  */
-bool float_mat_inv_4d(float invOut[16], float mat_in[16])
+bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4])
 {
   float_mat_adjoint_4d(invOut, mat_in);
 
@@ -869,8 +869,11 @@ bool float_mat_inv_4d(float invOut[16], float mat_in[16])
 
   float inv_det = 1.0f / det;
   int i;
-  for (i = 0; i < 16; ++i) {
-    invOut[i] = invOut[i] * inv_det;
+  for (i = 0; i < 4; ++i) {
+    invOut[0][i] = invOut[0][i] * inv_det;
+    invOut[1][i] = invOut[1][i] * inv_det;
+    invOut[2][i] = invOut[2][i] * inv_det;
+    invOut[3][i] = invOut[3][i] * inv_det;
   }
 
   return 0; //success
diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
index 545955125f..7dac5d2da0 100644
--- a/sw/airborne/math/pprz_algebra_float.h
+++ b/sw/airborne/math/pprz_algebra_float.h
@@ -883,7 +883,7 @@ static inline void float_mat_diagonal_scal(float **o, float v, int n)
 
 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]);
+extern bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4]);
 
 extern void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound);
 extern void float_vect3_bound_in_3d(struct FloatVect3 *vect3, float bound);
diff --git a/sw/simulator/nps/nps_sensors_utils.c b/sw/simulator/nps/nps_sensors_utils.c
index 684ffc12d9..2b580ecf86 100644
--- a/sw/simulator/nps/nps_sensors_utils.c
+++ b/sw/simulator/nps/nps_sensors_utils.c
@@ -3,12 +3,17 @@
 //#include <string.h>
 #include "math/pprz_algebra.h"
 
+// Compatibility with older glib
+#if !GLIB_CHECK_VERSION(2, 68, 0)
+#define g_memdup2 g_memdup
+#endif
+
 void UpdateSensorLatency(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading)
 {
   /* add new reading */
   struct BoozDatedSensor *cur_read = g_new(struct BoozDatedSensor, 1);
   cur_read->time = time;
-  cur_read->value = (struct DoubleVect3 *) g_memdup(cur_reading, sizeof(struct DoubleVect3));
+  cur_read->value = (struct DoubleVect3 *) g_memdup2(cur_reading, sizeof(struct DoubleVect3));
   *history = g_slist_prepend(*history, cur_read);
   /* remove old readings */
   GSList *last =  g_slist_last(*history);
@@ -30,7 +35,7 @@ void UpdateSensorLatency_Single(double time, gpointer cur_reading, GSList **hist
   /* add new reading */
   struct BoozDatedSensor_Single *cur_read = g_new(struct BoozDatedSensor_Single, 1);
   cur_read->time = time;
-  cur_read->value = (double *) g_memdup(cur_reading, sizeof(double));
+  cur_read->value = (double *) g_memdup2(cur_reading, sizeof(double));
   *history = g_slist_prepend(*history, cur_read);
   /* remove old readings */
   GSList *last =  g_slist_last(*history);