diff --git a/sw/airborne/pprz_algebra_float.h b/sw/airborne/pprz_algebra_float.h
index 815745505b..052b7b0984 100644
--- a/sw/airborne/pprz_algebra_float.h
+++ b/sw/airborne/pprz_algebra_float.h
@@ -90,6 +90,11 @@ struct FloatRates {
     vo.z = v1.x*v2.y - v1.y*v2.x;					\
   }
 
+#define FLOAT_VECT3_NORMALIZE(_v) {		\
+    const float n = FLOAT_VECT3_NORM(_v);	\
+    FLOAT_VECT3_SMUL(_v, 1./n, _v);		\
+  }
+
 /*
  * Rotation Matrices
  */
@@ -111,26 +116,26 @@ struct FloatRates {
   }
 
 /* initialises a rotation matrix from unit vector axis and angle */
-#define FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _ax, _an) {			\
+#define FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _uv, _an) {			\
     									\
-    const float ux2 = _rm.x*_rm.x;					\
-    const float uy2 = _rm.y*_rm.y;					\
-    const float uz2 = _rm.z*_rm.z;					\
-    const float uxuy = _rm.x*_rm.y;					\
-    const float uyuz = _rm.y*_rm.z;					\
-    const float uxuz = _rm.x*_rm.z;					\
-    const float can = cosf(_am);					\
-    const float san = sinf(_am);					\
+    const float ux2 = _uv.x*_uv.x;					\
+    const float uy2 = _uv.y*_uv.y;					\
+    const float uz2 = _uv.z*_uv.z;					\
+    const float uxuy = _uv.x*_uv.y;					\
+    const float uyuz = _uv.y*_uv.z;					\
+    const float uxuz = _uv.x*_uv.z;					\
+    const float can = cosf(_an);					\
+    const float san = sinf(_an);					\
     const float one_m_can = (1. - can);					\
     									\
     RMAT_ELMT(_rm, 0, 0) = ux2 + (1.-ux2)*can;				\
-    RMAT_ELMT(_rm, 0, 1) = uxuy*one_m_can - uz*san;			\
-    RMAT_ELMT(_rm, 0, 2) = uxuz*one_m_can + uy*san;			\
-    RMAT_ELMT(_rm, 1, 0) = uxuy*one_m_can + uz*san;			\
+    RMAT_ELMT(_rm, 0, 1) = uxuy*one_m_can + _uv.z*san;			\
+    RMAT_ELMT(_rm, 0, 2) = uxuz*one_m_can - _uv.y*san;			\
+    RMAT_ELMT(_rm, 1, 0) = uxuy*one_m_can - _uv.z*san;			\
     RMAT_ELMT(_rm, 1, 1) = uy2 + (1.-uy2)*can;				\
-    RMAT_ELMT(_rm, 1, 2) = uyuz*one_m_can-ux*san;			\
-    RMAT_ELMT(_rm, 2, 0) = uxuz*one_m_can-uy*san;			\
-    RMAT_ELMT(_rm, 2, 1) = uyuz*one_m_can+ux*san;			\
+    RMAT_ELMT(_rm, 1, 2) = uyuz*one_m_can + _uv.x*san;			\
+    RMAT_ELMT(_rm, 2, 0) = uxuz*one_m_can + _uv.y*san;			\
+    RMAT_ELMT(_rm, 2, 1) = uyuz*one_m_can - _uv.x*san;			\
     RMAT_ELMT(_rm, 2, 2) = uz2 + (1.-uz2)*can;				\
     									\
   }
@@ -334,6 +339,13 @@ struct FloatRates {
     									\
   }
 
+#define FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) {	\
+    const float san = sinf(_an/2.);			\
+    _q.qi = cosf(_an/2.);					\
+    _q.qx = san * _uv.x;				\
+    _q.qy = san * _uv.y;				\
+    _q.qz = san * _uv.z;				\
+  }
 
 /*
  *  Euler angles
diff --git a/sw/airborne/test/test_algebra.c b/sw/airborne/test/test_algebra.c
index 5ada9415ed..4662b17619 100644
--- a/sw/airborne/test/test_algebra.c
+++ b/sw/airborne/test/test_algebra.c
@@ -26,6 +26,7 @@ float test_quat_comp(struct FloatQuat qa2b_f, struct FloatQuat qb2c_f, int displ
 float test_rmat_comp_inv(struct FloatRMat ma2c_f, struct FloatRMat mb2c_f, int display);
 float test_quat_comp_inv(struct FloatQuat qa2c_f, struct FloatQuat qb2c_f, int display);
 
+void test_of_axis_angle(void);
 
 int main(int argc, char** argv) {
 
@@ -38,9 +39,10 @@ int main(int argc, char** argv) {
   //  test_4_float();
   printf("\n");
 
-  test_5();
-  test_6();
-  test_7();
+  //  test_5();
+  //test_6();
+  //test_7();
+  test_of_axis_angle();
   return 0;
 
 }
@@ -478,3 +480,61 @@ float test_quat_comp_inv(struct FloatQuat qa2c_f, struct FloatQuat qb2c_f, int d
   return norm_err;
 
 }
+
+
+
+void test_of_axis_angle(void) {
+
+  struct FloatVect3 axis = { 0., 1., 0.};
+  FLOAT_VECT3_NORMALIZE(axis);
+  DISPLAY_FLOAT_VECT3("axis", axis);
+  const float angle = RadOfDeg(30.);
+  printf("angle %f\n", DegOfRad(angle));
+
+  struct FloatQuat my_q;
+  FLOAT_QUAT_OF_AXIS_ANGLE(my_q, axis, angle);
+  DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("quat", my_q);
+
+  struct FloatMat33 my_r1;
+  FLOAT_RMAT_OF_QUAT(my_r1, my_q);
+  DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", my_r1);
+  DISPLAY_FLOAT_RMAT("rmat1", my_r1);
+
+  struct FloatMat33 my_r;
+  FLOAT_RMAT_OF_AXIS_ANGLE(my_r, axis, angle); 
+  DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", my_r);
+  DISPLAY_FLOAT_RMAT("rmat", my_r);
+
+  printf("\n");
+
+  struct FloatEulers eul = {RadOfDeg(30.), RadOfDeg(30.), 0.};
+
+  struct FloatVect3 uz = { 0., 0., 1.};
+  struct FloatMat33 r_yaw;
+  FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi);
+
+  struct FloatVect3 uy = { 0., 1., 0.};
+  struct FloatMat33 r_pitch;
+  FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta);
+
+  struct FloatVect3 ux = { 1., 0., 0.};
+  struct FloatMat33 r_roll;
+  FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi);
+
+  struct FloatMat33 r_yaw_pitch;
+  FLOAT_RMAT_COMP(r_yaw_pitch, r_yaw, r_pitch);
+
+  struct FloatMat33 r_yaw_pitch_roll;
+  FLOAT_RMAT_COMP(r_yaw_pitch_roll, r_yaw_pitch, r_roll);
+
+  DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", r_yaw_pitch_roll);
+  DISPLAY_FLOAT_RMAT("rmat", r_yaw_pitch_roll);
+
+  DISPLAY_FLOAT_EULERS_DEG("eul", eul);
+  struct FloatMat33 rmat1;
+  FLOAT_RMAT_OF_EULERS(rmat1, eul);
+  DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", rmat1);
+  DISPLAY_FLOAT_RMAT("rmat1", rmat1);
+  
+}
+
diff --git a/sw/logalizer/Makefile b/sw/logalizer/Makefile
index 2822ed7b84..01b8b79760 100644
--- a/sw/logalizer/Makefile
+++ b/sw/logalizer/Makefile
@@ -137,7 +137,7 @@ ahrs2fg: ahrs2fg.c network.c flight_gear.c utils.c
 	$(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS)
 
 test_samere: test_samere.c network.c flight_gear.c utils.c
-	$(CC) $(CFLAGS) -I../airborne -g -o $@ $^ $(LDFLAGS)
+	$(CC) $(CFLAGS) -I../airborne -I../airborne/test -g -o $@ $^ $(LDFLAGS)
 
 ivy_example: ivy_example.c
 	gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs`  `pcre-config --libs` -lglibivy