diff --git a/conf/airframes/examples/quadrotor_mlkf.xml b/conf/airframes/examples/quadrotor_mlkf.xml
new file mode 100644
index 0000000000..c4f1066f5f
--- /dev/null
+++ b/conf/airframes/examples/quadrotor_mlkf.xml
@@ -0,0 +1,228 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/settings/control/stabilization_att_float_quat.xml b/conf/settings/control/stabilization_att_float_quat.xml
new file mode 100644
index 0000000000..e2e3c886e8
--- /dev/null
+++ b/conf/settings/control/stabilization_att_float_quat.xml
@@ -0,0 +1,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
index ad506d136c..2ad5bc5d43 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
@@ -23,7 +23,7 @@
/**
* @file subsystems/ahrs/ahrs_float_mlkf.c
*
- * Mulitplicative linearized Kalman Filter in quaternion formulation.
+ * Multiplicative linearized Kalman Filter in quaternion formulation.
*
* Estimate the attitude, heading and gyro bias.
*/
@@ -46,6 +46,10 @@
//#include
+#ifndef AHRS_PROPAGATE_FREQUENCY
+#define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
+#endif
+
static inline void propagate_ref(void);
static inline void propagate_state(void);
static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, const float noise[]);
@@ -144,10 +148,12 @@ static inline void propagate_ref(void) {
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
- // printf("propagate_ref %f\n", gyro_float.p);
+
/* unbias measurement */
RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
+
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
+ /* lowpass angular rates */
const float alpha = 0.1;
FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate,
(1.-alpha), gyro_float, alpha);
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
index 5fa1b729f8..3b5552fcad 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
@@ -21,9 +21,9 @@
*/
/**
- * @file subsystems/ahrs/ahrs_float_mlkf.h
+ * @file subsystems/ahrs/ahrs_float_mlkf_opt.h
*
- * Mulitplicative linearized Kalman Filter in quaternion formulation.
+ * Multiplicative linearized Kalman Filter in quaternion formulation.
*
* Estimate the attitude, heading and gyro bias.
*/