diff --git a/sw/airborne/subsystems/ahrs.c b/sw/airborne/subsystems/ahrs.c
index 44e7cafb30..8bc6c38203 100644
--- a/sw/airborne/subsystems/ahrs.c
+++ b/sw/airborne/subsystems/ahrs.c
@@ -19,6 +19,12 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ahrs.c
+ * Attitude and Heading Reference System interface.
+ */
+
+
 #include "subsystems/ahrs.h"
 
 struct Ahrs ahrs;
diff --git a/sw/airborne/subsystems/ahrs.h b/sw/airborne/subsystems/ahrs.h
index 76c9438fbc..5c49dcf710 100644
--- a/sw/airborne/subsystems/ahrs.h
+++ b/sw/airborne/subsystems/ahrs.h
@@ -19,8 +19,9 @@
  * Boston, MA 02111-1307, USA.
  */
 
-/** \file ahrs.h
- * \brief Attitude and Heading Reference System interface
+/**
+ * @file subsystems/ahrs.h
+ * Attitude and Heading Reference System interface.
  */
 
 #ifndef AHRS_H
diff --git a/sw/airborne/subsystems/ahrs/ahrs_aligner.c b/sw/airborne/subsystems/ahrs/ahrs_aligner.c
index d853890df0..ee1ae63e14 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_aligner.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_aligner.c
@@ -19,6 +19,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ahrs/ahrs_aligner.c
+ *
+ * Low-pass IMU measurements at startup to align the AHRS.
+ *
+ */
+
 #include "ahrs_aligner.h"
 
 #include <stdlib.h> /* for abs() */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_aligner.h b/sw/airborne/subsystems/ahrs/ahrs_aligner.h
index 23c88c54ab..f5629e9fd7 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_aligner.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_aligner.h
@@ -19,6 +19,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ahrs/ahrs_aligner.h
+ *
+ * Interface to align the AHRS via low-passed measurements at startup.
+ *
+ */
+
 #ifndef AHRS_ALIGNER_H
 #define AHRS_ALIGNER_H
 
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
index aebfffa21f..3ec76ada56 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
@@ -19,6 +19,14 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ahrs/ahrs_float_cmpl.c
+ *
+ * Complementary filter in float to estimate the attitude, heading and gyro bias.
+ *
+ * Propagation can be done in rotation matrix or quaternion representation.
+ */
+
 #include "subsystems/ahrs.h"
 #include "subsystems/ahrs/ahrs_float_cmpl.h"
 #include "subsystems/ahrs/ahrs_float_utils.h"
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h
index 491ca8cdc1..a40d888f08 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h
@@ -19,6 +19,14 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ahrs/ahrs_float_cmpl.h
+ *
+ * Complementary filter in float to estimate the attitude, heading and gyro bias.
+ *
+ * Propagation can be done in rotation matrix or quaternion representation.
+ */
+
 #ifndef AHRS_FLOAT_CMPL
 #define AHRS_FLOAT_CMPL
 
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
index 2ace8a1c9e..0d5fcd415a 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
@@ -12,14 +12,17 @@
  *
  */
 
-/** \file ahrs_float_dcm.c
- *  \brief Attitude estimation for fixedwings based on the DCM
- *  Theory: http://code.google.com/p/gentlenav/downloads/list  file DCMDraft2.pdf
+/**
+ * @file subsystems/ahrs/ahrs_float_dcm.c
  *
- *  Options:
- *  -USE_HIGH_ACCEL_FLAG: no compensation when high accelerations present
- *  -USE_MAGNETOMETER_ONGROUND: use magnetic compensation before takeoff only while GPS course not good
- *  -USE_AHRS_GPS_ACCELERATIONS: forward acceleration compensation from GPS speed
+ * Attitude estimation for fixedwings based on the DCM.
+ *
+ * Theory: http://code.google.com/p/gentlenav/downloads/list  file DCMDraft2.pdf
+ *
+ * Options:
+ *  - USE_HIGH_ACCEL_FLAG: no compensation when high accelerations present
+ *  - USE_MAGNETOMETER_ONGROUND: use magnetic compensation before takeoff only while GPS course not good
+ *  - USE_AHRS_GPS_ACCELERATIONS: forward acceleration compensation from GPS speed
  *
  */
 
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
index 80ae0b1b13..0f92e10ce5 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
@@ -12,9 +12,12 @@
  *
  */
 
-/** \file ahrs_float_dcm.h
- *  \brief Attitude estimation for fixedwings based on the DCM
- *  Theory: http://code.google.com/p/gentlenav/downloads/list  file DCMDraft2.pdf
+/**
+ * @file subsystems/ahrs/ahrs_float_dcm.h
+ *
+ * Attitude estimation for fixedwings based on the DCM.
+ *
+ * Theory: http://code.google.com/p/gentlenav/downloads/list  file DCMDraft2.pdf
  *
  */
 
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_algebra.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_algebra.h
index c639ad12b1..9027a78175 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_algebra.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_algebra.h
@@ -1,4 +1,25 @@
-//Algebra helper functions for DCM
+/*
+ * Released under Creative Commons License
+ *
+ * 2010 The Paparazzi Team
+ *
+ *
+ * Based on Code by Jordi Munoz and William Premerlani, Supported by Chris Anderson (Wired) and Nathan Sindle (SparkFun).
+ * Version 1.0 for flat board updated by Doug Weibel and Jose Julio
+ *
+ */
+
+/**
+ * @file subsystems/ahrs/ahrs_float_dcm_algebra.h
+ *
+ * Algebra helper functions for DCM.
+ *
+ * @todo get rid of this and use pprz math lib.
+ */
+
+#ifndef AHRS_FLOAT_DCM_ALGEBRA_H
+#define AHRS_FLOAT_DCM_ALGEBRA_H
+
 
 static inline float Vector_Dot_Product(float vector1[3],float vector2[3])
 {
@@ -55,3 +76,5 @@ static inline void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
         }
     }
 }
+
+#endif // AHRS_FLOAT_DCM_ALGEBRA_H
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c
index 456fe2b455..e3e38086dc 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c
@@ -20,6 +20,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ahrs/ahrs_float_lkf.c
+ *
+ * Linearized Kalman Filter for attitude estimation.
+ *
+ */
+
 #include "ahrs_float_lkf.h"
 
 #include "subsystems/imu.h"
@@ -158,110 +165,110 @@ float bafl_R_mag;
 #endif
 
 
-#define FLOAT_MAT33_INVERT(_mo, _mi) {											\
-	float _det = 0.;															\
-	_det =   _mi[0][0] * (_mi[2][2] * _mi[1][1] - _mi[2][1] * _mi[1][2])		\
-	       - _mi[1][0] * (_mi[2][2] * _mi[0][1] - _mi[2][1] * _mi[0][2])		\
-	       + _mi[2][0] * (_mi[1][2] * _mi[0][1] - _mi[1][1]	* _mi[0][2]);		\
+#define FLOAT_MAT33_INVERT(_mo, _mi) {                                  \
+	float _det = 0.;                                                    \
+	_det =   _mi[0][0] * (_mi[2][2] * _mi[1][1] - _mi[2][1] * _mi[1][2]) \
+      - _mi[1][0] * (_mi[2][2] * _mi[0][1] - _mi[2][1] * _mi[0][2])		\
+      + _mi[2][0] * (_mi[1][2] * _mi[0][1] - _mi[1][1]	* _mi[0][2]);   \
 	if (_det != 0.) { /* ? test for zero? */							\
-		_mo[0][0] = (_mi[1][1] * _mi[2][2] - _mi[1][2] * _mi[2][1]) / _det;		\
-		_mo[0][1] = (_mi[0][2] * _mi[2][1] - _mi[0][1] * _mi[2][2]) / _det;		\
-		_mo[0][2] = (_mi[0][1] * _mi[1][2] - _mi[0][2] * _mi[1][1]) / _det;		\
-																				\
-		_mo[1][0] = (_mi[1][2] * _mi[2][0] - _mi[1][0] * _mi[2][2]) / _det;		\
-		_mo[1][1] = (_mi[0][0] * _mi[2][2] - _mi[0][2] * _mi[2][0]) / _det;		\
-		_mo[1][2] = (_mi[0][2] * _mi[1][0] - _mi[0][0] * _mi[1][2]) / _det;		\
-																				\
-		_mo[2][0] = (_mi[1][0] * _mi[2][1] - _mi[1][1] * _mi[2][0]) / _det;		\
-		_mo[2][1] = (_mi[0][1] * _mi[2][0] - _mi[0][0] * _mi[2][1]) / _det;		\
-		_mo[2][2] = (_mi[0][0] * _mi[1][1] - _mi[0][1] * _mi[1][0]) / _det;		\
-	} else {																	\
-		printf("ERROR! Not invertible!\n");										\
-		for (int _i=0; _i<3; _i++) {											\
-			for (int _j=0; _j<3; _j++) {										\
-				_mo[_i][_j] = 0.0;												\
-			}																	\
-		}																		\
-	}																			\
-}
+      _mo[0][0] = (_mi[1][1] * _mi[2][2] - _mi[1][2] * _mi[2][1]) / _det; \
+      _mo[0][1] = (_mi[0][2] * _mi[2][1] - _mi[0][1] * _mi[2][2]) / _det; \
+      _mo[0][2] = (_mi[0][1] * _mi[1][2] - _mi[0][2] * _mi[1][1]) / _det; \
+                                                                        \
+      _mo[1][0] = (_mi[1][2] * _mi[2][0] - _mi[1][0] * _mi[2][2]) / _det; \
+      _mo[1][1] = (_mi[0][0] * _mi[2][2] - _mi[0][2] * _mi[2][0]) / _det; \
+      _mo[1][2] = (_mi[0][2] * _mi[1][0] - _mi[0][0] * _mi[1][2]) / _det; \
+                                                                        \
+      _mo[2][0] = (_mi[1][0] * _mi[2][1] - _mi[1][1] * _mi[2][0]) / _det; \
+      _mo[2][1] = (_mi[0][1] * _mi[2][0] - _mi[0][0] * _mi[2][1]) / _det; \
+      _mo[2][2] = (_mi[0][0] * _mi[1][1] - _mi[0][1] * _mi[1][0]) / _det; \
+	} else {                                                            \
+      printf("ERROR! Not invertible!\n");                               \
+      for (int _i=0; _i<3; _i++) {                                      \
+        for (int _j=0; _j<3; _j++) {                                    \
+          _mo[_i][_j] = 0.0;                                            \
+        }                                                               \
+      }                                                                 \
+	}                                                                   \
+  }
 
-#define AHRS_TO_BFP() {												\
-	/* IMU rate */													\
-	RATES_BFP_OF_REAL(ahrs.imu_rate, bafl_rates);				\
-	/* LTP to IMU eulers      */									\
+#define AHRS_TO_BFP() {                                     \
+	/* IMU rate */                                          \
+	RATES_BFP_OF_REAL(ahrs.imu_rate, bafl_rates);           \
+	/* LTP to IMU eulers      */                            \
 	EULERS_BFP_OF_REAL(ahrs.ltp_to_imu_euler, bafl_eulers);	\
-	/* LTP to IMU quaternion */										\
-	QUAT_BFP_OF_REAL(ahrs.ltp_to_imu_quat, bafl_quat);			\
-	/* LTP to IMU rotation matrix */								\
-	RMAT_BFP_OF_REAL(ahrs.ltp_to_imu_rmat, bafl_dcm);			\
-}
+	/* LTP to IMU quaternion */                             \
+	QUAT_BFP_OF_REAL(ahrs.ltp_to_imu_quat, bafl_quat);      \
+	/* LTP to IMU rotation matrix */                        \
+	RMAT_BFP_OF_REAL(ahrs.ltp_to_imu_rmat, bafl_dcm);       \
+  }
 
-#define AHRS_LTP_TO_BODY() {																				\
-	/* Compute LTP to BODY quaternion */																	\
-	INT32_QUAT_COMP_INV(ahrs.ltp_to_body_quat, ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);	\
-	/* Compute LTP to BODY rotation matrix */																\
-	INT32_RMAT_COMP_INV(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);	\
-	/* compute LTP to BODY eulers */																		\
-	INT32_EULERS_OF_RMAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_rmat);							\
-	/* compute body rates */																				\
-	INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate);			\
-}
+#define AHRS_LTP_TO_BODY() {                                            \
+	/* Compute LTP to BODY quaternion */                                \
+    INT32_QUAT_COMP_INV(ahrs.ltp_to_body_quat, ahrs.ltp_to_imu_quat, imu.body_to_imu_quat); \
+	/* Compute LTP to BODY rotation matrix */                           \
+	INT32_RMAT_COMP_INV(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat); \
+	/* compute LTP to BODY eulers */                                    \
+	INT32_EULERS_OF_RMAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_rmat); \
+	/* compute body rates */                                            \
+	INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate); \
+  }
 
 
 void ahrs_init(void) {
-	int i, j;
+  int i, j;
 
-	for (i = 0; i < BAFL_SSIZE; i++) {
-		for (j = 0; j < BAFL_SSIZE; j++) {
-			bafl_T[i][j] = 0.;
-			bafl_P[i][j] = 0.;
-		}
-		/* Insert the diagonal elements of the T-Matrix. These will never change. */
-		bafl_T[i][i] = 1.0;
-		/* initial covariance values */
-		if (i<3) {
-			bafl_P[i][i] = 1.0;
-		} else {
-			bafl_P[i][i] = 0.1;
-		}
-	}
+  for (i = 0; i < BAFL_SSIZE; i++) {
+    for (j = 0; j < BAFL_SSIZE; j++) {
+      bafl_T[i][j] = 0.;
+      bafl_P[i][j] = 0.;
+    }
+    /* Insert the diagonal elements of the T-Matrix. These will never change. */
+    bafl_T[i][i] = 1.0;
+    /* initial covariance values */
+    if (i<3) {
+      bafl_P[i][i] = 1.0;
+    } else {
+      bafl_P[i][i] = 0.1;
+    }
+  }
 
-	FLOAT_QUAT_ZERO(bafl_quat);
+  FLOAT_QUAT_ZERO(bafl_quat);
 
-	ahrs.status = AHRS_UNINIT;
-	INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
-	INT_EULERS_ZERO(ahrs.ltp_to_imu_euler);
-	INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
-	INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat);
-	INT_RATES_ZERO(ahrs.body_rate);
-	INT_RATES_ZERO(ahrs.imu_rate);
+  ahrs.status = AHRS_UNINIT;
+  INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
+  INT_EULERS_ZERO(ahrs.ltp_to_imu_euler);
+  INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
+  INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat);
+  INT_RATES_ZERO(ahrs.body_rate);
+  INT_RATES_ZERO(ahrs.imu_rate);
 
-	ahrs_float_lkf_SetRaccel(BAFL_SIGMA_ACCEL);
-	ahrs_float_lkf_SetRmag(BAFL_SIGMA_MAG);
+  ahrs_float_lkf_SetRaccel(BAFL_SIGMA_ACCEL);
+  ahrs_float_lkf_SetRmag(BAFL_SIGMA_MAG);
 
-	bafl_Q_att = BAFL_Q_ATT;
-	bafl_Q_gyro = BAFL_Q_GYRO;
+  bafl_Q_att = BAFL_Q_ATT;
+  bafl_Q_gyro = BAFL_Q_GYRO;
 
-	FLOAT_VECT3_ASSIGN(bafl_h, BAFL_hx,BAFL_hy, BAFL_hz);
+  FLOAT_VECT3_ASSIGN(bafl_h, BAFL_hx,BAFL_hy, BAFL_hz);
 
 }
 
 void ahrs_align(void) {
-	RATES_FLOAT_OF_BFP(bafl_bias, ahrs_aligner.lp_gyro);
-	ACCELS_FLOAT_OF_BFP(bafl_accel_measure, ahrs_aligner.lp_accel);
-	ahrs.status = AHRS_RUNNING;
+  RATES_FLOAT_OF_BFP(bafl_bias, ahrs_aligner.lp_gyro);
+  ACCELS_FLOAT_OF_BFP(bafl_accel_measure, ahrs_aligner.lp_accel);
+  ahrs.status = AHRS_RUNNING;
 }
 
 static inline void ahrs_lowpass_accel(void) {
-	// get latest measurement
-	ACCELS_FLOAT_OF_BFP(bafl_accel_last, imu.accel);
-	// low pass it
-	VECT3_ADD(bafl_accel_measure, bafl_accel_last);
-	VECT3_SDIV(bafl_accel_measure, bafl_accel_measure, 2);
+  // get latest measurement
+  ACCELS_FLOAT_OF_BFP(bafl_accel_last, imu.accel);
+  // low pass it
+  VECT3_ADD(bafl_accel_measure, bafl_accel_last);
+  VECT3_SDIV(bafl_accel_measure, bafl_accel_measure, 2);
 
 #ifdef BAFL_DEBUG
-	bafl_phi_accel = atan2f( -bafl_accel_measure.y, -bafl_accel_measure.z);
-	bafl_theta_accel = atan2f(bafl_accel_measure.x, sqrtf(bafl_accel_measure.y*bafl_accel_measure.y + bafl_accel_measure.z*bafl_accel_measure.z));
+  bafl_phi_accel = atan2f( -bafl_accel_measure.y, -bafl_accel_measure.z);
+  bafl_theta_accel = atan2f(bafl_accel_measure.x, sqrtf(bafl_accel_measure.y*bafl_accel_measure.y + bafl_accel_measure.z*bafl_accel_measure.z));
 #endif
 }
 
@@ -283,123 +290,123 @@ static inline void ahrs_lowpass_accel(void) {
  *
  */
 void ahrs_propagate(void) {
-	int i, j, k;
+  int i, j, k;
 
-    ahrs_lowpass_accel();
+  ahrs_lowpass_accel();
 
-	/* compute unbiased rates */
-	RATES_FLOAT_OF_BFP(bafl_rates, imu.gyro);
-	RATES_SUB(bafl_rates, bafl_bias);
+  /* compute unbiased rates */
+  RATES_FLOAT_OF_BFP(bafl_rates, imu.gyro);
+  RATES_SUB(bafl_rates, bafl_bias);
 
 
-	/* run strapdown computation
-	 *
-	 */
+  /* run strapdown computation
+   *
+   */
 
-	/* multiplicative quaternion update */
-	/* compute correction quaternion */
-	QUAT_ASSIGN(bafl_qr, 1., bafl_rates.p * BAFL_DT / 2, bafl_rates.q * BAFL_DT / 2, bafl_rates.r * BAFL_DT / 2);
-	/* normalize it. maybe not necessary?? */
-	float q_sq;
-	q_sq = (bafl_qr.qx * bafl_qr.qx +bafl_qr.qy * bafl_qr.qy + bafl_qr.qz * bafl_qr.qz) / 4;
-	if (q_sq > 1) { /* this should actually never happen */
-		FLOAT_QUAT_SMUL(bafl_qr, bafl_qr, 1 / sqrtf(1 + q_sq));
-	} else {
-		bafl_qr.qi = sqrtf(1 - q_sq);
-	}
-	/* propagate correction quaternion */
-	FLOAT_QUAT_COMP(bafl_qtemp, bafl_quat, bafl_qr);
-	FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
+  /* multiplicative quaternion update */
+  /* compute correction quaternion */
+  QUAT_ASSIGN(bafl_qr, 1., bafl_rates.p * BAFL_DT / 2, bafl_rates.q * BAFL_DT / 2, bafl_rates.r * BAFL_DT / 2);
+  /* normalize it. maybe not necessary?? */
+  float q_sq;
+  q_sq = (bafl_qr.qx * bafl_qr.qx +bafl_qr.qy * bafl_qr.qy + bafl_qr.qz * bafl_qr.qz) / 4;
+  if (q_sq > 1) { /* this should actually never happen */
+    FLOAT_QUAT_SMUL(bafl_qr, bafl_qr, 1 / sqrtf(1 + q_sq));
+  } else {
+    bafl_qr.qi = sqrtf(1 - q_sq);
+  }
+  /* propagate correction quaternion */
+  FLOAT_QUAT_COMP(bafl_qtemp, bafl_quat, bafl_qr);
+  FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
 
-	bafl_qnorm = FLOAT_QUAT_NORM(bafl_quat);
-	//TODO check if broot force normalization is good, use lagrange normalization?
-	FLOAT_QUAT_NORMALIZE(bafl_quat);
+  bafl_qnorm = FLOAT_QUAT_NORM(bafl_quat);
+  //TODO check if broot force normalization is good, use lagrange normalization?
+  FLOAT_QUAT_NORMALIZE(bafl_quat);
 
 
-	/*
-	 *  compute all representations
-	 */
-	/* maintain rotation matrix representation */
-	FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
-	/* maintain euler representation */
-	FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
-	AHRS_TO_BFP();
-	AHRS_LTP_TO_BODY();
+  /*
+   *  compute all representations
+   */
+  /* maintain rotation matrix representation */
+  FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
+  /* maintain euler representation */
+  FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
+  AHRS_TO_BFP();
+  AHRS_LTP_TO_BODY();
 
 
-	/* error KF-Filter
-	 * propagate only the error covariance matrix since error is corrected after each measurement
-	 *
-	 * F = [ 0  0  0          ]
-	 *     [ 0  0  0   -Cbn   ]
-	 *     [ 0  0  0          ]
-	 *     [ 0  0  0  0  0  0 ]
-	 *     [ 0  0  0  0  0  0 ]
-	 *     [ 0  0  0  0  0  0 ]
-	 *
-	 * T = e^(dt * F)
-	 *
-	 * T = [ 1   0   0             ]
-	 *     [ 0   1   0   -Cbn*dt   ]
-	 *     [ 0   0   1             ]
-	 *     [ 0   0   0   1   0   0 ]
-	 *     [ 0   0   0   0   1   0 ]
-	 *     [ 0   0   0   0   0   1 ]
-	 *
-	 * P_prio = T * P * T_T + Q
-	 *
-	 */
+  /* error KF-Filter
+   * propagate only the error covariance matrix since error is corrected after each measurement
+   *
+   * F = [ 0  0  0          ]
+   *     [ 0  0  0   -Cbn   ]
+   *     [ 0  0  0          ]
+   *     [ 0  0  0  0  0  0 ]
+   *     [ 0  0  0  0  0  0 ]
+   *     [ 0  0  0  0  0  0 ]
+   *
+   * T = e^(dt * F)
+   *
+   * T = [ 1   0   0             ]
+   *     [ 0   1   0   -Cbn*dt   ]
+   *     [ 0   0   1             ]
+   *     [ 0   0   0   1   0   0 ]
+   *     [ 0   0   0   0   1   0 ]
+   *     [ 0   0   0   0   0   1 ]
+   *
+   * P_prio = T * P * T_T + Q
+   *
+   */
 
-	/*
-	 *  compute state transition matrix T
-	 *
-	 *  diagonal elements of T are always one
-	 */
-	for (i = 0; i < 3; i++) {
-		for (j = 0; j < 3; j++) {
-			bafl_T[i][j + 3] = -RMAT_ELMT(bafl_dcm, j, i); /* inverted bafl_dcm */
-		}
-	}
+  /*
+   *  compute state transition matrix T
+   *
+   *  diagonal elements of T are always one
+   */
+  for (i = 0; i < 3; i++) {
+    for (j = 0; j < 3; j++) {
+      bafl_T[i][j + 3] = -RMAT_ELMT(bafl_dcm, j, i); /* inverted bafl_dcm */
+    }
+  }
 
 
-	/*
-	 * estimate the a priori error covariance matrix P_k = T * P_k-1 * T_T + Q
-	 */
-	/* Temporary multiplication temp(6x6) = T(6x6) * P(6x6)      */
-	for (i = 0; i < BAFL_SSIZE; i++) {
-		for (j = 0; j < BAFL_SSIZE; j++) {
-			bafl_tempP[i][j] = 0.;
-			for (k = 0; k < BAFL_SSIZE; k++) {
-				bafl_tempP[i][j] += bafl_T[i][k] * bafl_P[k][j];
-			}
-		}
-	}
+  /*
+   * estimate the a priori error covariance matrix P_k = T * P_k-1 * T_T + Q
+   */
+  /* Temporary multiplication temp(6x6) = T(6x6) * P(6x6)      */
+  for (i = 0; i < BAFL_SSIZE; i++) {
+    for (j = 0; j < BAFL_SSIZE; j++) {
+      bafl_tempP[i][j] = 0.;
+      for (k = 0; k < BAFL_SSIZE; k++) {
+        bafl_tempP[i][j] += bafl_T[i][k] * bafl_P[k][j];
+      }
+    }
+  }
 
-	/* P_k(6x6) = temp(6x6) * T_T(6x6) + Q  */
-	for (i = 0; i < BAFL_SSIZE; i++) {
-		for (j = 0; j < BAFL_SSIZE; j++) {
-			bafl_P[i][j] = 0.;
-			for (k = 0; k < BAFL_SSIZE; k++) {
-				bafl_P[i][j] += bafl_tempP[i][k] * bafl_T[j][k]; //T[j][k] = T_T[k][j]
-			}
-		}
-		if (i<3) {
-			bafl_P[i][i] += bafl_Q_att;
-		} else {
-			bafl_P[i][i] += bafl_Q_gyro;
-		}
-	}
+  /* P_k(6x6) = temp(6x6) * T_T(6x6) + Q  */
+  for (i = 0; i < BAFL_SSIZE; i++) {
+    for (j = 0; j < BAFL_SSIZE; j++) {
+      bafl_P[i][j] = 0.;
+      for (k = 0; k < BAFL_SSIZE; k++) {
+        bafl_P[i][j] += bafl_tempP[i][k] * bafl_T[j][k]; //T[j][k] = T_T[k][j]
+      }
+    }
+    if (i<3) {
+      bafl_P[i][i] += bafl_Q_att;
+    } else {
+      bafl_P[i][i] += bafl_Q_gyro;
+    }
+  }
 
 #ifdef LKF_PRINT_P
-	printf("Pp =");
-	for (i = 0; i < 6; i++) {
-		printf("[");
-		for (j = 0; j < 6; j++) {
-			printf("%f\t", bafl_P[i][j]);
-		}
-		printf("]\n    ");
-	}
-	printf("\n");
+  printf("Pp =");
+  for (i = 0; i < 6; i++) {
+    printf("[");
+    for (j = 0; j < 6; j++) {
+      printf("%f\t", bafl_P[i][j]);
+    }
+    printf("]\n    ");
+  }
+  printf("\n");
 #endif
 }
 
@@ -408,229 +415,229 @@ void ahrs_update_accel(void) {
 }
 
 static void ahrs_do_update_accel(void) {
-	int i, j, k;
+  int i, j, k;
 
 #ifdef BAFL_DEBUG2
-	printf("Accel update.\n");
+  printf("Accel update.\n");
 #endif
 
-	/* P_prio = P */
-	for (i = 0; i < BAFL_SSIZE; i++) {
-		for (j = 0; j < BAFL_SSIZE; j++) {
-			bafl_Pprio[i][j] = bafl_P[i][j];
-		}
-	}
+  /* P_prio = P */
+  for (i = 0; i < BAFL_SSIZE; i++) {
+    for (j = 0; j < BAFL_SSIZE; j++) {
+      bafl_Pprio[i][j] = bafl_P[i][j];
+    }
+  }
 
-	/*
-	 * set up measurement matrix
-	 *
-	 * g = 9.81
-	 * gx = [ 0 -g  0
-	 *        g  0  0
-	 *        0  0  0 ]
-	 * H = [          0 0 0 ]
-	 *	   [ -Cnb*gx  0 0 0 ]
-	 *     [          0 0 0 ]
-	 *
-	 * */
-	bafl_H[0][0] = -RMAT_ELMT(bafl_dcm, 0, 1) * BAFL_g;
-	bafl_H[0][1] =  RMAT_ELMT(bafl_dcm, 0, 0) * BAFL_g;
-	bafl_H[1][0] = -RMAT_ELMT(bafl_dcm, 1, 1) * BAFL_g;
-	bafl_H[1][1] =  RMAT_ELMT(bafl_dcm, 1, 0) * BAFL_g;
-	bafl_H[2][0] = -RMAT_ELMT(bafl_dcm, 2, 1) * BAFL_g;
-	bafl_H[2][1] =  RMAT_ELMT(bafl_dcm, 2, 0) * BAFL_g;
-	/* rest is zero
-	bafl_H[0][2] = 0.;
-	bafl_H[1][2] = 0.;
-	bafl_H[2][2] = 0.;*/
+  /*
+   * set up measurement matrix
+   *
+   * g = 9.81
+   * gx = [ 0 -g  0
+   *        g  0  0
+   *        0  0  0 ]
+   * H = [          0 0 0 ]
+   *	   [ -Cnb*gx  0 0 0 ]
+   *     [          0 0 0 ]
+   *
+   * */
+  bafl_H[0][0] = -RMAT_ELMT(bafl_dcm, 0, 1) * BAFL_g;
+  bafl_H[0][1] =  RMAT_ELMT(bafl_dcm, 0, 0) * BAFL_g;
+  bafl_H[1][0] = -RMAT_ELMT(bafl_dcm, 1, 1) * BAFL_g;
+  bafl_H[1][1] =  RMAT_ELMT(bafl_dcm, 1, 0) * BAFL_g;
+  bafl_H[2][0] = -RMAT_ELMT(bafl_dcm, 2, 1) * BAFL_g;
+  bafl_H[2][1] =  RMAT_ELMT(bafl_dcm, 2, 0) * BAFL_g;
+  /* rest is zero
+     bafl_H[0][2] = 0.;
+     bafl_H[1][2] = 0.;
+     bafl_H[2][2] = 0.;*/
 
-	/**********************************************
-	 * compute Kalman gain K
-	 *
-	 * K = P_prio * H_T * inv(S)
-	 * S = H * P_prio * HT + R
-	 *
-	 * K = P_prio * HT * inv(H * P_prio * HT + R)
-	 *
-	 **********************************************/
+  /**********************************************
+   * compute Kalman gain K
+   *
+   * K = P_prio * H_T * inv(S)
+   * S = H * P_prio * HT + R
+   *
+   * K = P_prio * HT * inv(H * P_prio * HT + R)
+   *
+   **********************************************/
 
-	/* covariance residual S = H * P_prio * HT + R    */
+  /* covariance residual S = H * P_prio * HT + R    */
 
-	/* temp_S(3x6) = H(3x6) * P_prio(6x6)
-	 * last 4 columns of H are zero
-	 */
+  /* temp_S(3x6) = H(3x6) * P_prio(6x6)
+   * last 4 columns of H are zero
+   */
+  for (i = 0; i < 3; i++) {
+    for (j = 0; j < BAFL_SSIZE; j++) {
+      bafl_tempS[i][j]  = bafl_H[i][0] * bafl_Pprio[0][j];
+      bafl_tempS[i][j] += bafl_H[i][1] * bafl_Pprio[1][j];
+    }
+  }
+
+  /* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
+   *
+   * bottom 4 rows of HT are zero
+   */
+  for (i = 0; i < 3; i++) {
+    for (j = 0; j < 3; j++) {
+      bafl_S[i][j]  = bafl_tempS[i][0] * bafl_H[j][0]; /* H[j][0] = HT[0][j] */
+      bafl_S[i][j] += bafl_tempS[i][1] * bafl_H[j][1]; /* H[j][0] = HT[0][j] */
+    }
+    bafl_S[i][i] += bafl_R_accel;
+  }
+
+  /* invert S
+   */
+  FLOAT_MAT33_INVERT(bafl_invS, bafl_S);
+
+
+  /* temp_K(6x3) = P_prio(6x6) * HT(6x3)
+   *
+   * bottom 4 rows of HT are zero
+   */
+  for (i = 0; i < BAFL_SSIZE; i++) {
+    for (j = 0; j < 3; j++) {
+      /* not computing zero elements */
+      bafl_tempK[i][j]  = bafl_Pprio[i][0] * bafl_H[j][0]; /* H[j][0] = HT[0][j] */
+      bafl_tempK[i][j] += bafl_Pprio[i][1] * bafl_H[j][1]; /* H[j][1] = HT[1][j] */
+    }
+  }
+
+  /* K(6x3) = temp_K(6x3) * invS(3x3)
+   */
+  for (i = 0; i < BAFL_SSIZE; i++) {
+    for (j = 0; j < 3; j++) {
+      bafl_K[i][j]  = bafl_tempK[i][0] * bafl_invS[0][j];
+      bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j];
+      bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j];
+    }
+  }
+
+  /**********************************************
+   * Update filter state.
+   *
+   *  The a priori filter state is zero since the errors are corrected after each update.
+   *
+   *  X = X_prio + K (y - H * X_prio)
+   *  X = K * y
+   **********************************************/
+
+  /*printf("R = ");
 	for (i = 0; i < 3; i++) {
-		for (j = 0; j < BAFL_SSIZE; j++) {
-			bafl_tempS[i][j]  = bafl_H[i][0] * bafl_Pprio[0][j];
-			bafl_tempS[i][j] += bafl_H[i][1] * bafl_Pprio[1][j];
-		}
-	}
-
-	/* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
-	 *
-	 * bottom 4 rows of HT are zero
-	 */
-	for (i = 0; i < 3; i++) {
-		for (j = 0; j < 3; j++) {
-			bafl_S[i][j]  = bafl_tempS[i][0] * bafl_H[j][0]; /* H[j][0] = HT[0][j] */
-			bafl_S[i][j] += bafl_tempS[i][1] * bafl_H[j][1]; /* H[j][0] = HT[0][j] */
-		}
-		bafl_S[i][i] += bafl_R_accel;
-	}
-
-	/* invert S
-	 */
-	FLOAT_MAT33_INVERT(bafl_invS, bafl_S);
-
-
-	/* temp_K(6x3) = P_prio(6x6) * HT(6x3)
-	 *
-	 * bottom 4 rows of HT are zero
-	 */
-	for (i = 0; i < BAFL_SSIZE; i++) {
-		for (j = 0; j < 3; j++) {
-			/* not computing zero elements */
-			bafl_tempK[i][j]  = bafl_Pprio[i][0] * bafl_H[j][0]; /* H[j][0] = HT[0][j] */
-			bafl_tempK[i][j] += bafl_Pprio[i][1] * bafl_H[j][1]; /* H[j][1] = HT[1][j] */
-		}
-	}
-
-	/* K(6x3) = temp_K(6x3) * invS(3x3)
-	 */
-	for (i = 0; i < BAFL_SSIZE; i++) {
-		for (j = 0; j < 3; j++) {
-			bafl_K[i][j]  = bafl_tempK[i][0] * bafl_invS[0][j];
-			bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j];
-			bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j];
-		}
-	}
-
-	/**********************************************
-	 * Update filter state.
-	 *
-	 *  The a priori filter state is zero since the errors are corrected after each update.
-	 *
-	 *  X = X_prio + K (y - H * X_prio)
-	 *  X = K * y
-	 **********************************************/
-
-	/*printf("R = ");
-	for (i = 0; i < 3; i++) {
-		printf("[");
-		for (j = 0; j < 3; j++) {
-			printf("%f\t", RMAT_ELMT(bafl_dcm, i, j));
-		}
-		printf("]\n    ");
+    printf("[");
+    for (j = 0; j < 3; j++) {
+    printf("%f\t", RMAT_ELMT(bafl_dcm, i, j));
+    }
+    printf("]\n    ");
 	}
 	printf("\n");*/
 
-	/* innovation
-	 * y = Cnb * -[0; 0; g] - accel
-	 */
-	bafl_ya.x = -RMAT_ELMT(bafl_dcm, 0, 2) * BAFL_g - bafl_accel_measure.x;
-	bafl_ya.y = -RMAT_ELMT(bafl_dcm, 1, 2) * BAFL_g - bafl_accel_measure.y;
-	bafl_ya.z = -RMAT_ELMT(bafl_dcm, 2, 2) * BAFL_g - bafl_accel_measure.z;
+  /* innovation
+   * y = Cnb * -[0; 0; g] - accel
+   */
+  bafl_ya.x = -RMAT_ELMT(bafl_dcm, 0, 2) * BAFL_g - bafl_accel_measure.x;
+  bafl_ya.y = -RMAT_ELMT(bafl_dcm, 1, 2) * BAFL_g - bafl_accel_measure.y;
+  bafl_ya.z = -RMAT_ELMT(bafl_dcm, 2, 2) * BAFL_g - bafl_accel_measure.z;
 
-	/* X(6) = K(6x3) * y(3)
-	 */
-	for (i = 0; i < BAFL_SSIZE; i++) {
-		bafl_X[i]  = bafl_K[i][0] * bafl_ya.x;
-		bafl_X[i] += bafl_K[i][1] * bafl_ya.y;
-		bafl_X[i] += bafl_K[i][2] * bafl_ya.z;
-	}
+  /* X(6) = K(6x3) * y(3)
+   */
+  for (i = 0; i < BAFL_SSIZE; i++) {
+    bafl_X[i]  = bafl_K[i][0] * bafl_ya.x;
+    bafl_X[i] += bafl_K[i][1] * bafl_ya.y;
+    bafl_X[i] += bafl_K[i][2] * bafl_ya.z;
+  }
 
-	/**********************************************
-	 * Update the filter covariance.
-	 *
-	 *  P = P_prio - K * H * P_prio
-	 *  P = ( I - K * H ) * P_prio
-	 *
-	 **********************************************/
+  /**********************************************
+   * Update the filter covariance.
+   *
+   *  P = P_prio - K * H * P_prio
+   *  P = ( I - K * H ) * P_prio
+   *
+   **********************************************/
 
-	/*  temp(6x6) = I(6x6) - K(6x3)*H(3x6)
-	 *
-	 *  last 4 columns of H are zero
-	 */
-	for (i = 0; i < BAFL_SSIZE; i++) {
-		for (j = 0; j < BAFL_SSIZE; j++) {
-			if (i == j) {
-				bafl_tempP[i][j] = 1.;
-			} else {
-				bafl_tempP[i][j] = 0.;
-			}
-			if (j < 2) { /* omit the parts where H is zero */
-				for (k = 0; k < 3; k++) {
-					bafl_tempP[i][j] -= bafl_K[i][k] * bafl_H[k][j];
-				}
-			}
-		}
-	}
+  /*  temp(6x6) = I(6x6) - K(6x3)*H(3x6)
+   *
+   *  last 4 columns of H are zero
+   */
+  for (i = 0; i < BAFL_SSIZE; i++) {
+    for (j = 0; j < BAFL_SSIZE; j++) {
+      if (i == j) {
+        bafl_tempP[i][j] = 1.;
+      } else {
+        bafl_tempP[i][j] = 0.;
+      }
+      if (j < 2) { /* omit the parts where H is zero */
+        for (k = 0; k < 3; k++) {
+          bafl_tempP[i][j] -= bafl_K[i][k] * bafl_H[k][j];
+        }
+      }
+    }
+  }
 
-	/*  P(6x6) = temp(6x6) * P_prio(6x6)
-	 */
-	for (i = 0; i < BAFL_SSIZE; i++) {
-		for (j = 0; j < BAFL_SSIZE; j++) {
-			bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j];
-			for (k = 1; k < BAFL_SSIZE; k++) {
-				bafl_P[i][j] += bafl_tempP[i][k] * bafl_Pprio[k][j];
-			}
-		}
-	}
+  /*  P(6x6) = temp(6x6) * P_prio(6x6)
+   */
+  for (i = 0; i < BAFL_SSIZE; i++) {
+    for (j = 0; j < BAFL_SSIZE; j++) {
+      bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j];
+      for (k = 1; k < BAFL_SSIZE; k++) {
+        bafl_P[i][j] += bafl_tempP[i][k] * bafl_Pprio[k][j];
+      }
+    }
+  }
 
 #ifdef LKF_PRINT_P
-	printf("Pua=");
-	for (i = 0; i < 6; i++) {
-		printf("[");
-		for (j = 0; j < 6; j++) {
-			printf("%f\t", bafl_P[i][j]);
-		}
-		printf("]\n    ");
-	}
-	printf("\n");
+  printf("Pua=");
+  for (i = 0; i < 6; i++) {
+    printf("[");
+    for (j = 0; j < 6; j++) {
+      printf("%f\t", bafl_P[i][j]);
+    }
+    printf("]\n    ");
+  }
+  printf("\n");
 #endif
 
-	/**********************************************
-	 *  Correct errors.
-	 *
-	 *
-	 **********************************************/
+  /**********************************************
+   *  Correct errors.
+   *
+   *
+   **********************************************/
 
-	/*  Error quaternion.
-	 */
-	QUAT_ASSIGN(bafl_q_a_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
-	FLOAT_QUAT_INVERT(bafl_q_a_err, bafl_q_a_err);
+  /*  Error quaternion.
+   */
+  QUAT_ASSIGN(bafl_q_a_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
+  FLOAT_QUAT_INVERT(bafl_q_a_err, bafl_q_a_err);
 
-	/* normalize
-	 * Is this needed? Or is the approximation good enough?*/
-	float q_sq;
-	q_sq = bafl_q_a_err.qx * bafl_q_a_err.qx + bafl_q_a_err.qy * bafl_q_a_err.qy + bafl_q_a_err.qz * bafl_q_a_err.qz;
-	if (q_sq > 1) { /* this should actually never happen */
-		FLOAT_QUAT_SMUL(bafl_q_a_err, bafl_q_a_err, 1 / sqrtf(1 + q_sq));
-		printf("Accel error quaternion q_sq > 1!!\n");
-	} else {
-		bafl_q_a_err.qi = sqrtf(1 - q_sq);
-	}
+  /* normalize
+   * Is this needed? Or is the approximation good enough?*/
+  float q_sq;
+  q_sq = bafl_q_a_err.qx * bafl_q_a_err.qx + bafl_q_a_err.qy * bafl_q_a_err.qy + bafl_q_a_err.qz * bafl_q_a_err.qz;
+  if (q_sq > 1) { /* this should actually never happen */
+    FLOAT_QUAT_SMUL(bafl_q_a_err, bafl_q_a_err, 1 / sqrtf(1 + q_sq));
+    printf("Accel error quaternion q_sq > 1!!\n");
+  } else {
+    bafl_q_a_err.qi = sqrtf(1 - q_sq);
+  }
 
-	/*  correct attitude
-	 */
-	FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_a_err, bafl_quat);
-	FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
+  /*  correct attitude
+   */
+  FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_a_err, bafl_quat);
+  FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
 
 
-	/*  correct gyro bias
-	 */
-	RATES_ASSIGN(bafl_b_a_err, bafl_X[3], bafl_X[4], bafl_X[5]);
-	RATES_SUB(bafl_bias, bafl_b_a_err);
+  /*  correct gyro bias
+   */
+  RATES_ASSIGN(bafl_b_a_err, bafl_X[3], bafl_X[4], bafl_X[5]);
+  RATES_SUB(bafl_bias, bafl_b_a_err);
 
-	/*
-	 *  compute all representations
-	 */
-	/* maintain rotation matrix representation */
-	FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
-	/* maintain euler representation */
-	FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
-	AHRS_TO_BFP();
-	AHRS_LTP_TO_BODY();
+  /*
+   *  compute all representations
+   */
+  /* maintain rotation matrix representation */
+  FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
+  /* maintain euler representation */
+  FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
+  AHRS_TO_BFP();
+  AHRS_LTP_TO_BODY();
 }
 
 
@@ -639,212 +646,212 @@ void ahrs_update_mag(void) {
 }
 
 static void ahrs_do_update_mag(void) {
-	int i, j, k;
+  int i, j, k;
 
 #ifdef BAFL_DEBUG2
-	printf("Mag update.\n");
+  printf("Mag update.\n");
 #endif
 
-	MAGS_FLOAT_OF_BFP(bafl_mag, imu.mag);
+  MAGS_FLOAT_OF_BFP(bafl_mag, imu.mag);
 
-	/* P_prio = P */
-	for (i = 0; i < BAFL_SSIZE; i++) {
-		for (j = 0; j < BAFL_SSIZE; j++) {
-			bafl_Pprio[i][j] = bafl_P[i][j];
-		}
-	}
+  /* P_prio = P */
+  for (i = 0; i < BAFL_SSIZE; i++) {
+    for (j = 0; j < BAFL_SSIZE; j++) {
+      bafl_Pprio[i][j] = bafl_P[i][j];
+    }
+  }
 
-	/*
-	 * set up measurement matrix
-	 *
-	 * h = [236.; -2.; 396.];
-	 * hx = [ h(2); -h(1); 0]; //only psi (phi and theta = 0)
-	 * Hm = Cnb * hx;
-	 * H = [ 0  0        0  0  0
-	 *       0  0 Cnb*hx 0  0  0
-	 *       0  0        0  0  0 ];
-	 * */
-	/*bafl_H[0][2] = RMAT_ELMT(bafl_dcm, 0, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 0, 1) * bafl_h.x;
+  /*
+   * set up measurement matrix
+   *
+   * h = [236.; -2.; 396.];
+   * hx = [ h(2); -h(1); 0]; //only psi (phi and theta = 0)
+   * Hm = Cnb * hx;
+   * H = [ 0  0        0  0  0
+   *       0  0 Cnb*hx 0  0  0
+   *       0  0        0  0  0 ];
+   * */
+  /*bafl_H[0][2] = RMAT_ELMT(bafl_dcm, 0, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 0, 1) * bafl_h.x;
 	bafl_H[1][2] = RMAT_ELMT(bafl_dcm, 1, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 1, 1) * bafl_h.x;
 	bafl_H[2][2] = RMAT_ELMT(bafl_dcm, 2, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 2, 1) * bafl_h.x;*/
-	bafl_H[0][2] = -RMAT_ELMT(bafl_dcm, 0, 1);
-	bafl_H[1][2] = -RMAT_ELMT(bafl_dcm, 1, 1);
-	bafl_H[2][2] = -RMAT_ELMT(bafl_dcm, 2, 1);
-	//rest is zero
+  bafl_H[0][2] = -RMAT_ELMT(bafl_dcm, 0, 1);
+  bafl_H[1][2] = -RMAT_ELMT(bafl_dcm, 1, 1);
+  bafl_H[2][2] = -RMAT_ELMT(bafl_dcm, 2, 1);
+  //rest is zero
 
-	/**********************************************
-	 * compute Kalman gain K
-	 *
-	 * K = P_prio * H_T * inv(S)
-	 * S = H * P_prio * HT + R
-	 *
-	 * K = P_prio * HT * inv(H * P_prio * HT + R)
-	 *
-	 **********************************************/
+  /**********************************************
+   * compute Kalman gain K
+   *
+   * K = P_prio * H_T * inv(S)
+   * S = H * P_prio * HT + R
+   *
+   * K = P_prio * HT * inv(H * P_prio * HT + R)
+   *
+   **********************************************/
 
-	/* covariance residual S = H * P_prio * HT + R    */
+  /* covariance residual S = H * P_prio * HT + R    */
 
-	/* temp_S(3x6) = H(3x6) * P_prio(6x6)
-	 *
-	 * only third column of H is non-zero
-	 */
-	for (i = 0; i < 3; i++) {
-		for (j = 0; j < BAFL_SSIZE; j++) {
-			bafl_tempS[i][j] = bafl_H[i][2] * bafl_Pprio[2][j];
-		}
-	}
+  /* temp_S(3x6) = H(3x6) * P_prio(6x6)
+   *
+   * only third column of H is non-zero
+   */
+  for (i = 0; i < 3; i++) {
+    for (j = 0; j < BAFL_SSIZE; j++) {
+      bafl_tempS[i][j] = bafl_H[i][2] * bafl_Pprio[2][j];
+    }
+  }
 
-	/* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
-	 *
-	 * only third row of HT is non-zero
-	 */
-	for (i = 0; i < 3; i++) {
-		for (j = 0; j < 3; j++) {
-			bafl_S[i][j] = bafl_tempS[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */
-		}
-		bafl_S[i][i] += bafl_R_mag;
-	}
+  /* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
+   *
+   * only third row of HT is non-zero
+   */
+  for (i = 0; i < 3; i++) {
+    for (j = 0; j < 3; j++) {
+      bafl_S[i][j] = bafl_tempS[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */
+    }
+    bafl_S[i][i] += bafl_R_mag;
+  }
 
-	/* invert S
-	 */
-	FLOAT_MAT33_INVERT(bafl_invS, bafl_S);
+  /* invert S
+   */
+  FLOAT_MAT33_INVERT(bafl_invS, bafl_S);
 
-	/* temp_K(6x3) = P_prio(6x6) * HT(6x3)
-	 *
-	 * only third row of HT is non-zero
-	 */
-	for (i = 0; i < BAFL_SSIZE; i++) {
-		for (j = 0; j < 3; j++) {
-			/* not computing zero elements */
-			bafl_tempK[i][j] = bafl_Pprio[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */
-		}
-	}
+  /* temp_K(6x3) = P_prio(6x6) * HT(6x3)
+   *
+   * only third row of HT is non-zero
+   */
+  for (i = 0; i < BAFL_SSIZE; i++) {
+    for (j = 0; j < 3; j++) {
+      /* not computing zero elements */
+      bafl_tempK[i][j] = bafl_Pprio[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */
+    }
+  }
 
-	/* K(6x3) = temp_K(6x3) * invS(3x3)
-	 */
-	for (i = 0; i < BAFL_SSIZE; i++) {
-		for (j = 0; j < 3; j++) {
-			bafl_K[i][j]  = bafl_tempK[i][0] * bafl_invS[0][j];
-			bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j];
-			bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j];
-		}
-	}
+  /* K(6x3) = temp_K(6x3) * invS(3x3)
+   */
+  for (i = 0; i < BAFL_SSIZE; i++) {
+    for (j = 0; j < 3; j++) {
+      bafl_K[i][j]  = bafl_tempK[i][0] * bafl_invS[0][j];
+      bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j];
+      bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j];
+    }
+  }
 
-	/**********************************************
-	 * Update filter state.
-	 *
-	 *  The a priori filter state is zero since the errors are corrected after each update.
-	 *
-	 *  X = X_prio + K (y - H * X_prio)
-	 *  X = K * y
-	 **********************************************/
+  /**********************************************
+   * Update filter state.
+   *
+   *  The a priori filter state is zero since the errors are corrected after each update.
+   *
+   *  X = X_prio + K (y - H * X_prio)
+   *  X = K * y
+   **********************************************/
 
-	/*  innovation
-	 *  y = Cnb * [hx; hy; hz] - mag
-	 */
-	FLOAT_RMAT_VECT3_MUL(bafl_ym, bafl_dcm, bafl_h); //can be optimized
-	FLOAT_VECT3_SUB(bafl_ym, bafl_mag);
+  /*  innovation
+   *  y = Cnb * [hx; hy; hz] - mag
+   */
+  FLOAT_RMAT_VECT3_MUL(bafl_ym, bafl_dcm, bafl_h); //can be optimized
+  FLOAT_VECT3_SUB(bafl_ym, bafl_mag);
 
-	/* X(6) = K(6x3) * y(3)
-	 */
-	for (i = 0; i < BAFL_SSIZE; i++) {
-		bafl_X[i]  = bafl_K[i][0] * bafl_ym.x;
-		bafl_X[i] += bafl_K[i][1] * bafl_ym.y;
-		bafl_X[i] += bafl_K[i][2] * bafl_ym.z;
-	}
+  /* X(6) = K(6x3) * y(3)
+   */
+  for (i = 0; i < BAFL_SSIZE; i++) {
+    bafl_X[i]  = bafl_K[i][0] * bafl_ym.x;
+    bafl_X[i] += bafl_K[i][1] * bafl_ym.y;
+    bafl_X[i] += bafl_K[i][2] * bafl_ym.z;
+  }
 
-	/**********************************************
-	 * Update the filter covariance.
-	 *
-	 *  P = P_prio - K * H * P_prio
-	 *  P = ( I - K * H ) * P_prio
-	 *
-	 **********************************************/
+  /**********************************************
+   * Update the filter covariance.
+   *
+   *  P = P_prio - K * H * P_prio
+   *  P = ( I - K * H ) * P_prio
+   *
+   **********************************************/
 
-	/*  temp(6x6) = I(6x6) - K(6x3)*H(3x6)
-	 *
-	 *  last 3 columns of H are zero
-	 */
-	for (i = 0; i < 6; i++) {
-		for (j = 0; j < 6; j++) {
-			if (i == j) {
-				bafl_tempP[i][j] = 1.;
-			} else {
-				bafl_tempP[i][j] = 0.;
-			}
-			if (j == 2) { /* omit the parts where H is zero */
-				for (k = 0; k < 3; k++) {
-					bafl_tempP[i][j] -= bafl_K[i][k] * bafl_H[k][j];
-				}
-			}
-		}
-	}
+  /*  temp(6x6) = I(6x6) - K(6x3)*H(3x6)
+   *
+   *  last 3 columns of H are zero
+   */
+  for (i = 0; i < 6; i++) {
+    for (j = 0; j < 6; j++) {
+      if (i == j) {
+        bafl_tempP[i][j] = 1.;
+      } else {
+        bafl_tempP[i][j] = 0.;
+      }
+      if (j == 2) { /* omit the parts where H is zero */
+        for (k = 0; k < 3; k++) {
+          bafl_tempP[i][j] -= bafl_K[i][k] * bafl_H[k][j];
+        }
+      }
+    }
+  }
 
-	/*  P(6x6) = temp(6x6) * P_prio(6x6)
-	 */
-	for (i = 0; i < BAFL_SSIZE; i++) {
-		for (j = 0; j < BAFL_SSIZE; j++) {
-			bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j];
-			for (k = 1; k < BAFL_SSIZE; k++) {
-				bafl_P[i][j] += bafl_tempP[i][k] * bafl_Pprio[k][j];
-			}
-		}
-	}
+  /*  P(6x6) = temp(6x6) * P_prio(6x6)
+   */
+  for (i = 0; i < BAFL_SSIZE; i++) {
+    for (j = 0; j < BAFL_SSIZE; j++) {
+      bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j];
+      for (k = 1; k < BAFL_SSIZE; k++) {
+        bafl_P[i][j] += bafl_tempP[i][k] * bafl_Pprio[k][j];
+      }
+    }
+  }
 
 #ifdef LKF_PRINT_P
-	printf("Pum=");
-	for (i = 0; i < 6; i++) {
-		printf("[");
-		for (j = 0; j < 6; j++) {
-			printf("%f\t", bafl_P[i][j]);
-		}
-		printf("]\n    ");
-	}
-	printf("\n");
+  printf("Pum=");
+  for (i = 0; i < 6; i++) {
+    printf("[");
+    for (j = 0; j < 6; j++) {
+      printf("%f\t", bafl_P[i][j]);
+    }
+    printf("]\n    ");
+  }
+  printf("\n");
 #endif
 
-	/**********************************************
-	 *  Correct errors.
-	 *
-	 *
-	 **********************************************/
+  /**********************************************
+   *  Correct errors.
+   *
+   *
+   **********************************************/
 
-	/*  Error quaternion.
-	 */
-	QUAT_ASSIGN(bafl_q_m_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
-	FLOAT_QUAT_INVERT(bafl_q_m_err, bafl_q_m_err);
-	/* normalize */
-	float q_sq;
-	q_sq = bafl_q_m_err.qx * bafl_q_m_err.qx + bafl_q_m_err.qy * bafl_q_m_err.qy + bafl_q_m_err.qz * bafl_q_m_err.qz;
-	if (q_sq > 1) { /* this should actually never happen */
-		FLOAT_QUAT_SMUL(bafl_q_m_err, bafl_q_m_err, 1 / sqrtf(1 + q_sq));
-		printf("mag error quaternion q_sq > 1!!\n");
-	} else {
-		bafl_q_m_err.qi = sqrtf(1 - q_sq);
-	}
+  /*  Error quaternion.
+   */
+  QUAT_ASSIGN(bafl_q_m_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
+  FLOAT_QUAT_INVERT(bafl_q_m_err, bafl_q_m_err);
+  /* normalize */
+  float q_sq;
+  q_sq = bafl_q_m_err.qx * bafl_q_m_err.qx + bafl_q_m_err.qy * bafl_q_m_err.qy + bafl_q_m_err.qz * bafl_q_m_err.qz;
+  if (q_sq > 1) { /* this should actually never happen */
+    FLOAT_QUAT_SMUL(bafl_q_m_err, bafl_q_m_err, 1 / sqrtf(1 + q_sq));
+    printf("mag error quaternion q_sq > 1!!\n");
+  } else {
+    bafl_q_m_err.qi = sqrtf(1 - q_sq);
+  }
 
-	/*  correct attitude
-	 */
-	FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_m_err, bafl_quat);
-	FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
+  /*  correct attitude
+   */
+  FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_m_err, bafl_quat);
+  FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
 
-	/*  correct gyro bias
-	 */
-	RATES_ASSIGN(bafl_b_m_err, bafl_X[3], bafl_X[4], bafl_X[5]);
-	RATES_SUB(bafl_bias, bafl_b_m_err);
+  /*  correct gyro bias
+   */
+  RATES_ASSIGN(bafl_b_m_err, bafl_X[3], bafl_X[4], bafl_X[5]);
+  RATES_SUB(bafl_bias, bafl_b_m_err);
 
-	/*
-	 *  compute all representations
-	 */
-	/* maintain rotation matrix representation */
-	FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
-	/* maintain euler representation */
-	FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
-	AHRS_TO_BFP();
-	AHRS_LTP_TO_BODY();
+  /*
+   *  compute all representations
+   */
+  /* maintain rotation matrix representation */
+  FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
+  /* maintain euler representation */
+  FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
+  AHRS_TO_BFP();
+  AHRS_LTP_TO_BODY();
 }
 
 void ahrs_update(void) {
-	ahrs_update_accel();
-	ahrs_update_mag();
+  ahrs_update_accel();
+  ahrs_update_mag();
 }
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.h b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.h
index 1fb091860c..71afc6d3cf 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.h
@@ -20,6 +20,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ahrs/ahrs_float_lkf.h
+ *
+ * Linearized Kalman Filter for attitude estimation.
+ *
+ */
+
 #ifndef AHRS_FLOAT_LKF_H
 #define AHRS_FLOAT_LKF_H
 
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
index 2db36ce6c9..9aed07595d 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
@@ -1,3 +1,32 @@
+/*
+ * Copyright (C) 2009 Felix Ruess <felix.ruess@gmail.com>
+ * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/ahrs/ahrs_float_utils.h
+ *
+ * Utility functions for floating point AHRS implementations.
+ *
+ */
+
 #ifndef AHRS_FLOAT_UTILS_H
 #define AHRS_FLOAT_UTILS_H
 
diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.c b/sw/airborne/subsystems/ahrs/ahrs_infrared.c
index e3c2a5f0b3..40128ed7fc 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_infrared.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.c
@@ -19,6 +19,16 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ahrs/ahrs_infrared.c
+ *
+ * Attitude estimation using infrared sensors detecting the horizon.
+ * For fixedwings only:
+ * - GPS course is used as heading.
+ * - ADC channels can be used for gyros.
+ *
+ */
+
 #include "subsystems/ahrs/ahrs_infrared.h"
 
 #include "subsystems/sensors/infrared.h"
diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.h b/sw/airborne/subsystems/ahrs/ahrs_infrared.h
index defcb61d7f..0ff3cd83f2 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_infrared.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.h
@@ -19,6 +19,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ahrs/ahrs_infrared.h
+ *
+ * Fixedwing attitude estimation using infrared sensors.
+ *
+ */
+
 #ifndef AHRS_INFRARED_H
 #define AHRS_INFRARED_H
 
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
index c5a56a3bf6..02d31d6d7b 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
@@ -19,6 +19,15 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ahrs/ahrs_int_cmpl_euler.c
+ *
+ * Complementary filter in euler representation (fixed-point).
+ *
+ * Estimate the attitude, heading and gyro bias.
+ *
+ */
+
 #include "ahrs_int_cmpl_euler.h"
 
 #include "state.h"
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
index 9210793a28..8ad70b2147 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
@@ -19,6 +19,14 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ahrs/ahrs_int_cmpl_euler.h
+ *
+ * Complementary filter in euler representation (fixed-point).
+ *
+ */
+
+
 #ifndef AHRS_INT_CMPL_EULER_H
 #define AHRS_INT_CMPL_EULER_H
 
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
index 47571dacd3..7db726ec22 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
@@ -19,6 +19,14 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ahrs/ahrs_int_cmpl_quat.c
+ *
+ * Quaternion complementary filter (fixed-point).
+ *
+ * Estimate the attitude, heading and gyro bias.
+ *
+ */
 
 #include "subsystems/ahrs/ahrs_int_cmpl_quat.h"
 #include "subsystems/ahrs/ahrs_aligner.h"
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h
index 6d3b131c02..a71c60e058 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h
@@ -19,6 +19,15 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ahrs/ahrs_int_cmpl_quat.h
+ *
+ * Quaternion complementary filter (fixed-point).
+ *
+ * Estimate the attitude, heading and gyro bias.
+ *
+ */
+
 #ifndef AHRS_INT_CMPL_H
 #define AHRS_INT_CMPL_H
 
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h
index 37bf830569..d1f930848e 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h
@@ -1,3 +1,32 @@
+/*
+ * Copyright (C) 2009 Felix Ruess <felix.ruess@gmail.com>
+ * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/ahrs/ahrs_int_utils.h
+ *
+ * Utility functions for fixed point AHRS implementations.
+ *
+ */
+
 #ifndef AHRS_INT_UTILS_H
 #define AHRS_INT_UTILS_H
 
diff --git a/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h b/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h
index b548ea3b04..35db048f9e 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h
@@ -1,3 +1,6 @@
+#ifndef AHRS_MAGNETIC_FIELD_MODEL_H
+#define AHRS_MAGNETIC_FIELD_MODEL_H
+
 #include "generated/airframe.h"
 
 #ifndef AHRS_H_X
@@ -8,3 +11,4 @@
 #define AHRS_H_Y 0
 #endif
 
+#endif
diff --git a/sw/airborne/subsystems/ahrs/ahrs_sim.c b/sw/airborne/subsystems/ahrs/ahrs_sim.c
index 112a9fb9ab..3c3b0df5fa 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_sim.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_sim.c
@@ -19,6 +19,12 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ahrs/ahrs_sim.c
+ *
+ * Dummy plug to set the AHRS from the simple OCaml simulator.
+ *
+ */
 
 #include "subsystems/ahrs.h"
 #include "subsystems/ahrs/ahrs_sim.h"
diff --git a/sw/airborne/subsystems/ahrs/ahrs_sim.h b/sw/airborne/subsystems/ahrs/ahrs_sim.h
index 8d4146587e..0a79b17d41 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_sim.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_sim.h
@@ -19,6 +19,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ahrs/ahrs_sim.h
+ *
+ * Interface to set the AHRS from the simple OCaml simulator.
+ *
+ */
+
 #ifndef AHRS_SIM_H
 #define AHRS_SIM_H
 
diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c
index a4f30efb68..36666c8bbe 100644
--- a/sw/airborne/subsystems/imu.c
+++ b/sw/airborne/subsystems/imu.c
@@ -19,7 +19,8 @@
  * Boston, MA 02111-1307, USA.
  */
 
-/** @file imu.c
+/**
+ * @file subsystems/imu.c
  * Inertial Measurement Unit interface.
  */
 
diff --git a/sw/airborne/subsystems/imu.h b/sw/airborne/subsystems/imu.h
index dd6badfe3c..72f759f7ef 100644
--- a/sw/airborne/subsystems/imu.h
+++ b/sw/airborne/subsystems/imu.h
@@ -18,8 +18,10 @@
  * the Free Software Foundation, 59 Temple Place - Suite 330,
  * Boston, MA 02111-1307, USA.
  */
-/** \file imu.h
- * \brief Inertial Measurement Unit interface
+
+/**
+ * @file subsystems/imu.h
+ * Inertial Measurement Unit interface.
  */
 
 #ifndef IMU_H
diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c
index 0eb0a55e02..f42a85ac2d 100644
--- a/sw/airborne/subsystems/ins.c
+++ b/sw/airborne/subsystems/ins.c
@@ -19,6 +19,12 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ins.c
+ * Integrated Navigation System interface.
+ */
+
+
 #include "subsystems/ins.h"
 
 struct Ins ins;
diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h
index 40f9e0eaf6..918dc07050 100644
--- a/sw/airborne/subsystems/ins.h
+++ b/sw/airborne/subsystems/ins.h
@@ -19,6 +19,11 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ins.h
+ * Integrated Navigation System interface.
+ */
+
 #ifndef INS_H
 #define INS_H
 
diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c
index 2c1bfadf44..0af2ea4569 100644
--- a/sw/airborne/subsystems/ins/hf_float.c
+++ b/sw/airborne/subsystems/ins/hf_float.c
@@ -20,6 +20,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ins/hf_float.c
+ *
+ * Horizontal filter (x,y) to estimate position and velocity.
+ *
+ */
+
 #include "subsystems/ins/hf_float.h"
 #include "subsystems/ins.h"
 #include "subsystems/imu.h"
diff --git a/sw/airborne/subsystems/ins/hf_float.h b/sw/airborne/subsystems/ins/hf_float.h
index 6dd3ad02d7..2d1c08dbc9 100644
--- a/sw/airborne/subsystems/ins/hf_float.h
+++ b/sw/airborne/subsystems/ins/hf_float.h
@@ -19,6 +19,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ins/hf_float.h
+ *
+ * Horizontal filter (x,y) to estimate position and velocity.
+ *
+ */
+
 #ifndef HF_FLOAT_H
 #define HF_FLOAT_H
 
diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c
index 4e927a31e1..86f47b80ec 100644
--- a/sw/airborne/subsystems/ins/ins_alt_float.c
+++ b/sw/airborne/subsystems/ins/ins_alt_float.c
@@ -19,8 +19,9 @@
  * Boston, MA 02111-1307, USA.
  */
 
-/** @file ins_alt_float.c
- * Filters altitude and climb rate.
+/**
+ * @file subsystems/ins/ins_alt_float.c
+ * Filters altitude and climb rate for fixedwings.
  */
 
 #include "subsystems/ins.h"
diff --git a/sw/airborne/subsystems/ins/ins_alt_float.h b/sw/airborne/subsystems/ins/ins_alt_float.h
index 9f6824493c..958dcd6b62 100644
--- a/sw/airborne/subsystems/ins/ins_alt_float.h
+++ b/sw/airborne/subsystems/ins/ins_alt_float.h
@@ -18,7 +18,11 @@
  * along with paparazzi; see the file COPYING.  If not, write to
  * the Free Software Foundation, 59 Temple Place - Suite 330,
  * Boston, MA 02111-1307, USA.
- *
+ */
+
+/**
+ * @file subsystems/ins/ins_alt_float.h
+ * Filters altitude and climb rate for fixedwings.
  */
 
 #ifndef INS_ALT_FLOAT_H
diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c
index 51625cada0..c274b32024 100644
--- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c
+++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c
@@ -19,7 +19,9 @@
  * Boston, MA 02111-1307, USA.
  */
 
-/** @file ins_gps_passthrough.c
+/**
+ * @file subsystems/ins/ins_gps_passthrough.c
+ *
  * Simply passes GPS position and velocity through to the state interface.
  */
 
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index 41b037cbd3..f5bd9e217f 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -19,6 +19,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ins/ins_int.c
+ *
+ * INS for rotorcrafts combining vertical and horizontal filters.
+ *
+ */
+
 #include "subsystems/ins/ins_int.h"
 
 #include "subsystems/imu.h"
diff --git a/sw/airborne/subsystems/ins/ins_int.h b/sw/airborne/subsystems/ins/ins_int.h
index 6699d45f16..4361913e8e 100644
--- a/sw/airborne/subsystems/ins/ins_int.h
+++ b/sw/airborne/subsystems/ins/ins_int.h
@@ -19,6 +19,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ins/ins_int.h
+ *
+ * INS for rotorcrafts combining vertical and horizontal filters.
+ *
+ */
+
 #ifndef INS_INT_H
 #define INS_INT_H
 
diff --git a/sw/airborne/subsystems/ins/ins_int_extended.c b/sw/airborne/subsystems/ins/ins_int_extended.c
index 64c97707f6..6c7b7eddf5 100644
--- a/sw/airborne/subsystems/ins/ins_int_extended.c
+++ b/sw/airborne/subsystems/ins/ins_int_extended.c
@@ -20,6 +20,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ins/ins_int_extended.c
+ *
+ * INS for rotorcrafts combining vertical and horizontal filters.
+ *
+ */
+
 #include "subsystems/ins/ins_int.h"
 
 #include "subsystems/imu.h"
diff --git a/sw/airborne/subsystems/ins/vf_extended_float.c b/sw/airborne/subsystems/ins/vf_extended_float.c
index 646d34af05..48d96f0b09 100644
--- a/sw/airborne/subsystems/ins/vf_extended_float.c
+++ b/sw/airborne/subsystems/ins/vf_extended_float.c
@@ -20,6 +20,15 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ins/vf_extended_float.c
+ *
+ * Extended vertical filter (in float).
+ *
+ * Estimates altitude, vertical speed, accelerometer bias
+ * and barometer offset.
+ */
+
 #include "subsystems/ins/vf_extended_float.h"
 
 #define DEBUG_VFF_EXTENDED 1
diff --git a/sw/airborne/subsystems/ins/vf_extended_float.h b/sw/airborne/subsystems/ins/vf_extended_float.h
index 3f8b93b75e..119c5100d4 100644
--- a/sw/airborne/subsystems/ins/vf_extended_float.h
+++ b/sw/airborne/subsystems/ins/vf_extended_float.h
@@ -20,6 +20,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ins/vf_extended_float.h
+ *
+ * Interface for extended vertical filter (in float).
+ *
+ */
+
 #ifndef VF_EXTENDED_FLOAT_H
 #define VF_EXTENDED_FLOAT_H
 
diff --git a/sw/airborne/subsystems/ins/vf_float.c b/sw/airborne/subsystems/ins/vf_float.c
index 0b52df40db..54bf0c612b 100644
--- a/sw/airborne/subsystems/ins/vf_float.c
+++ b/sw/airborne/subsystems/ins/vf_float.c
@@ -19,6 +19,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ins/vf_float.c
+ *
+ * Vertical filter (in float) estimating altitude, velocity and accel bias.
+ *
+ */
+
 #include "subsystems/ins/vf_float.h"
 
 /*
@@ -131,7 +138,7 @@ void vff_propagate(float accel) {
   // update covariance
   Pp = Pm - K*H*Pm;
 */
-__attribute__ ((always_inline)) static inline void update_z_conf(float z_meas, float conf) {
+static inline void update_z_conf(float z_meas, float conf) {
   vff_z_meas = z_meas;
 
   const float y = z_meas - vff_z;
@@ -188,7 +195,7 @@ void vff_update_z_conf(float z_meas, float conf) {
   // update covariance
   Pp = Pm - K*H*Pm;
 */
-__attribute__ ((always_inline)) static inline void update_vz_conf(float vz, float conf) {
+static inline void update_vz_conf(float vz, float conf) {
   const float yd = vz - vff_zdot;
   const float S = vff_P[1][1] + conf;
   const float K1 = vff_P[0][1] * 1/S;
diff --git a/sw/airborne/subsystems/ins/vf_float.h b/sw/airborne/subsystems/ins/vf_float.h
index 1c81494882..b98e6affd1 100644
--- a/sw/airborne/subsystems/ins/vf_float.h
+++ b/sw/airborne/subsystems/ins/vf_float.h
@@ -19,6 +19,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ins/vf_float.h
+ *
+ * Vertical filter (in float) estimating altitude, velocity and accel bias.
+ *
+ */
+
 #ifndef VF_FLOAT_H
 #define VF_FLOAT_H
 
diff --git a/sw/airborne/subsystems/ins/vf_int.c b/sw/airborne/subsystems/ins/vf_int.c
index 511b19e4a9..a269cd51c0 100644
--- a/sw/airborne/subsystems/ins/vf_int.c
+++ b/sw/airborne/subsystems/ins/vf_int.c
@@ -19,6 +19,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ins/vf_int.c
+ *
+ * Vertical filter (fixed-point) estimating altitude, velocity and accel bias.
+ *
+ */
+
 #include "subsystems/ins/vf_int.h"
 
 #include "booz_geometry_mixed.h"
diff --git a/sw/airborne/subsystems/ins/vf_int.h b/sw/airborne/subsystems/ins/vf_int.h
index bc48552d39..0da923e11b 100644
--- a/sw/airborne/subsystems/ins/vf_int.h
+++ b/sw/airborne/subsystems/ins/vf_int.h
@@ -19,6 +19,13 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/ins/vf_int.h
+ *
+ * Vertical filter (fixed-point) estimating altitude, velocity and accel bias.
+ *
+ */
+
 #ifndef VF_INT_H
 #define VF_INT_H