Angular rate recovery

Examples/Advanced/main.c

@@ -28,8 +28,9 @@ int main() {
     const FusionAhrsSettings settings = {
             .convention = FusionConventionNwu,
             .gain = 0.5f,
+            .gyroscopeRange = 2000.0f, /* replace this with actual gyroscope range in degrees/s */
             .accelerationRejection = 10.0f,
-            .magneticRejection = 20.0f,
+            .magneticRejection = 10.0f,
             .recoveryTriggerPeriod = 5 * SAMPLE_RATE, /* 5 seconds */
     };
     FusionAhrsSetSettings(&ahrs, &settings);

Fusion/FusionAhrs.c

@@ -47,6 +47,7 @@ void FusionAhrsInitialise(FusionAhrs *const ahrs) {
     const FusionAhrsSettings settings = {
             .convention = FusionConventionNwu,
             .gain = 0.5f,
+            .gyroscopeRange = 0.0f,
             .accelerationRejection = 90.0f,
             .magneticRejection = 90.0f,
             .recoveryTriggerPeriod = 0,
@@ -65,6 +66,7 @@ void FusionAhrsReset(FusionAhrs *const ahrs) {
     ahrs->accelerometer = FUSION_VECTOR_ZERO;
     ahrs->initialising = true;
     ahrs->rampedGain = INITIAL_GAIN;
+    ahrs->angularRateRecovery = false;
     ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO;
     ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO;
     ahrs->accelerometerIgnored = false;
@@ -83,12 +85,13 @@ void FusionAhrsReset(FusionAhrs *const ahrs) {
 void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings) {
     ahrs->settings.convention = settings->convention;
     ahrs->settings.gain = settings->gain;
+    ahrs->settings.gyroscopeRange = settings->gyroscopeRange == 0.0f ? FLT_MAX : 0.98f * settings->gyroscopeRange;
     ahrs->settings.accelerationRejection = settings->accelerationRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2);
     ahrs->settings.magneticRejection = settings->magneticRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2);
     ahrs->settings.recoveryTriggerPeriod = settings->recoveryTriggerPeriod;
     ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
     ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
-    if ((settings->gain == 0.0f) || (settings->recoveryTriggerPeriod == 0)) {
+    if ((settings->gain == 0.0f) || (settings->recoveryTriggerPeriod == 0)) { // disable acceleration and magnetic rejection features if gain is zero
         ahrs->settings.accelerationRejection = FLT_MAX;
         ahrs->settings.magneticRejection = FLT_MAX;
     }
@@ -113,12 +116,21 @@ void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, cons
     // Store accelerometer
     ahrs->accelerometer = accelerometer;
 
+    // Reinitialise if gyroscope range exceeded
+    if ((fabs(gyroscope.axis.x) > ahrs->settings.gyroscopeRange) || (fabs(gyroscope.axis.y) > ahrs->settings.gyroscopeRange) || (fabs(gyroscope.axis.z) > ahrs->settings.gyroscopeRange)) {
+        const FusionQuaternion quaternion = ahrs->quaternion;
+        FusionAhrsReset(ahrs);
+        ahrs->quaternion = quaternion;
+        ahrs->angularRateRecovery = true;
+    }
+
     // Ramp down gain during initialisation
     if (ahrs->initialising == true) {
         ahrs->rampedGain -= ahrs->rampedGainStep * deltaTime;
         if ((ahrs->rampedGain < ahrs->settings.gain) || (ahrs->settings.gain == 0.0f)) {
             ahrs->rampedGain = ahrs->settings.gain;
             ahrs->initialising = false;
+            ahrs->angularRateRecovery = false;
         }
     }
 
@@ -461,6 +473,7 @@ FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahr
 FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs) {
     const FusionAhrsFlags flags = {
             .initialising = ahrs->initialising,
+            .angularRateRecovery = ahrs->angularRateRecovery,
             .accelerationRecovery = ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout,
             .magneticRecovery= ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout,
     };

Fusion/FusionAhrs.h

@@ -24,6 +24,7 @@
 typedef struct {
     FusionConvention convention;
     float gain;
+    float gyroscopeRange;
     float accelerationRejection;
     float magneticRejection;
     unsigned int recoveryTriggerPeriod;
@@ -40,6 +41,7 @@ typedef struct {
     bool initialising;
     float rampedGain;
     float rampedGainStep;
+    bool angularRateRecovery;
     FusionVector halfAccelerometerFeedback;
     FusionVector halfMagnetometerFeedback;
     bool accelerometerIgnored;
@@ -67,6 +69,7 @@ typedef struct {
  */
 typedef struct {
     bool initialising;
+    bool angularRateRecovery;
     bool accelerationRecovery;
     bool magneticRecovery;
 } FusionAhrsFlags;

Python/Python-C-API/Flags.h

@@ -18,6 +18,10 @@ static PyObject *flags_get_initialising(Flags *self) {
     return build_bool(self->flags.initialising);
 }
 
+static PyObject *flags_get_angular_rate_recovery(Flags *self) {
+    return build_bool(self->flags.angularRateRecovery);
+}
+
 static PyObject *flags_get_acceleration_recovery(Flags *self) {
     return build_bool(self->flags.accelerationRecovery);
 }
@@ -28,6 +32,7 @@ static PyObject *flags_get_magnetic_recovery(Flags *self) {
 
 static PyGetSetDef flags_get_set[] = {
         {"initialising",          (getter) flags_get_initialising,          NULL, "", NULL},
+        {"angular_rate_recovery", (getter) flags_get_angular_rate_recovery, NULL, "", NULL},
         {"acceleration_recovery", (getter) flags_get_acceleration_recovery, NULL, "", NULL},
         {"magnetic_recovery",     (getter) flags_get_magnetic_recovery,     NULL, "", NULL},
         {NULL}  /* sentinel */

Python/Python-C-API/Settings.h

@@ -13,7 +13,7 @@ typedef struct {
 static PyObject *settings_new(PyTypeObject *subtype, PyObject *args, PyObject *keywords) {
     Settings *const self = (Settings *) subtype->tp_alloc(subtype, 0);
 
-    const char *const error = PARSE_TUPLE(args, "ifffI", &self->settings.convention, &self->settings.gain, &self->settings.accelerationRejection, &self->settings.magneticRejection, &self->settings.recoveryTriggerPeriod);
+    const char *const error = PARSE_TUPLE(args, "iffffI", &self->settings.convention, &self->settings.gain, &self->settings.gyroscopeRange, &self->settings.accelerationRejection, &self->settings.magneticRejection, &self->settings.recoveryTriggerPeriod);
     if (error != NULL) {
         PyErr_SetString(PyExc_TypeError, error);
         return NULL;
@@ -55,6 +55,21 @@ static int settings_set_gain(Settings *self, PyObject *value, void *closure) {
     return 0;
 }
 
+static PyObject *settings_get_gyroscope_range(Settings *self) {
+    return Py_BuildValue("f", self->settings.gyroscopeRange);
+}
+
+static int settings_set_gyroscope_range(Settings *self, PyObject *value, void *closure) {
+    const float gyroscope_range = (float) PyFloat_AsDouble(value);
+
+    if (PyErr_Occurred()) {
+        return -1;
+    }
+
+    self->settings.gyroscopeRange = gyroscope_range;
+    return 0;
+}
+
 static PyObject *settings_get_acceleration_rejection(Settings *self) {
     return Py_BuildValue("f", self->settings.accelerationRejection);
 }
@@ -103,6 +118,7 @@ static int settings_set_recovery_trigger_period(Settings *self, PyObject *value,
 static PyGetSetDef settings_get_set[] = {
         {"convention",              (getter) settings_get_convention,              (setter) settings_set_convention,              "", NULL},
         {"gain",                    (getter) settings_get_gain,                    (setter) settings_set_gain,                    "", NULL},
+        {"gyroscope_range",         (getter) settings_get_gyroscope_range,         (setter) settings_set_gyroscope_range,         "", NULL},
         {"acceleration_rejection",  (getter) settings_get_acceleration_rejection,  (setter) settings_set_acceleration_rejection,  "", NULL},
         {"magnetic_rejection",      (getter) settings_get_magnetic_rejection,      (setter) settings_set_magnetic_rejection,      "", NULL},
         {"recovery_trigger_period", (getter) settings_get_recovery_trigger_period, (setter) settings_set_recovery_trigger_period, "", NULL},

Python/advanced_example.py

@@ -19,16 +19,17 @@
 
 ahrs.settings = imufusion.Settings(imufusion.CONVENTION_NWU,  # convention
                                    0.5,  # gain
+                                   2000,  # gyroscope range
                                    10,  # acceleration rejection
-                                   20,  # magnetic rejection
+                                   10,  # magnetic rejection
                                    5 * sample_rate)  # recovery trigger period = 5 seconds
 
 # Process sensor data
 delta_time = numpy.diff(timestamp, prepend=timestamp[0])
 
 euler = numpy.empty((len(timestamp), 3))
 internal_states = numpy.empty((len(timestamp), 6))
-flags = numpy.empty((len(timestamp), 3))
+flags = numpy.empty((len(timestamp), 4))
 
 for index in range(len(timestamp)):
     gyroscope[index] = offset.update(gyroscope[index])
@@ -47,6 +48,7 @@
 
     ahrs_flags = ahrs.flags
     flags[index] = numpy.array([ahrs_flags.initialising,
+                                ahrs_flags.angular_rate_recovery,
                                 ahrs_flags.acceleration_recovery,
                                 ahrs_flags.magnetic_recovery])
 
@@ -60,7 +62,7 @@ def plot_bool(axis, x, y, label):
 
 
 # Plot Euler angles
-figure, axes = pyplot.subplots(nrows=10, sharex=True, gridspec_kw={"height_ratios": [6, 1, 2, 1, 1, 1, 2, 1, 1, 1]})
+figure, axes = pyplot.subplots(nrows=11, sharex=True, gridspec_kw={"height_ratios": [6, 1, 1, 2, 1, 1, 1, 2, 1, 1, 1]})
 
 figure.suptitle("Euler angles, internal states, and flags")
 
@@ -74,33 +76,36 @@ def plot_bool(axis, x, y, label):
 # Plot initialising flag
 plot_bool(axes[1], timestamp, flags[:, 0], "Initialising")
 
+# Plot angular rate recovery flag
+plot_bool(axes[2], timestamp, flags[:, 1], "Angular rate recovery")
+
 # Plot acceleration rejection internal states and flags
-axes[2].plot(timestamp, internal_states[:, 0], "tab:olive", label="Acceleration error")
-axes[2].set_ylabel("Degrees")
-axes[2].grid()
-axes[2].legend()
+axes[3].plot(timestamp, internal_states[:, 0], "tab:olive", label="Acceleration error")
+axes[3].set_ylabel("Degrees")
+axes[3].grid()
+axes[3].legend()
 
-plot_bool(axes[3], timestamp, internal_states[:, 1], "Accelerometer ignored")
+plot_bool(axes[4], timestamp, internal_states[:, 1], "Accelerometer ignored")
 
-axes[4].plot(timestamp, internal_states[:, 2], "tab:orange", label="Acceleration recovery trigger")
-axes[4].grid()
-axes[4].legend()
+axes[5].plot(timestamp, internal_states[:, 2], "tab:orange", label="Acceleration recovery trigger")
+axes[5].grid()
+axes[5].legend()
 
-plot_bool(axes[5], timestamp, flags[:, 1], "Acceleration recovery")
+plot_bool(axes[6], timestamp, flags[:, 2], "Acceleration recovery")
 
 # Plot magnetic rejection internal states and flags
-axes[6].plot(timestamp, internal_states[:, 3], "tab:olive", label="Magnetic error")
-axes[6].set_ylabel("Degrees")
-axes[6].grid()
-axes[6].legend()
+axes[7].plot(timestamp, internal_states[:, 3], "tab:olive", label="Magnetic error")
+axes[7].set_ylabel("Degrees")
+axes[7].grid()
+axes[7].legend()
 
-plot_bool(axes[7], timestamp, internal_states[:, 4], "Magnetometer ignored")
+plot_bool(axes[8], timestamp, internal_states[:, 4], "Magnetometer ignored")
 
-axes[8].plot(timestamp, internal_states[:, 5], "tab:orange", label="Magnetic recovery trigger")
-axes[8].grid()
-axes[8].legend()
+axes[9].plot(timestamp, internal_states[:, 5], "tab:orange", label="Magnetic recovery trigger")
+axes[9].grid()
+axes[9].legend()
 
-plot_bool(axes[9], timestamp, flags[:, 2], "Magnetic recovery")
+plot_bool(axes[10], timestamp, flags[:, 3], "Magnetic recovery")
 
 if len(sys.argv) == 1:  # don't show plots when script run by CI
     pyplot.show()

README.md

@@ -18,7 +18,11 @@ The algorithm calculates the orientation as the integration of the gyroscope sum
 
 ### Initialisation
 
-Initialisation occurs when the algorithm starts for the first time.  During initialisation, the acceleration and magnetic rejection features are disabled and the gain is ramped down from 10 to the final value over a 3 second period.  This allows the measurement of orientation to rapidly converges from an arbitrary initial value to the value indicated by the sensors.  The algorithm outputs should be regarded as unreliable during initialisation.
+Initialisation occurs when the algorithm starts for the first time and during angular rate recovery.  During initialisation, the acceleration and magnetic rejection features are disabled and the gain is ramped down from 10 to the final value over a 3 second period.  This allows the measurement of orientation to rapidly converge from an arbitrary initial value to the value indicated by the sensors.
+
+### Angular rate recovery
+
+Angular rates that exceed the gyroscope measurement range cannot be tracked and will trigger an angular rate recovery.  Angular rate recovery is activated when the angular rate exceeds the 98% of the gyroscope measurement range and equivalent to a reinitialisation of the algorithm. 
 
 ### Acceleration rejection
 
@@ -38,13 +42,14 @@ The algorithm provides three outputs: quaternion, linear acceleration, and Earth
 
 The AHRS algorithm settings are defined by the `FusionAhrsSettings` structure and set using the `FusionAhrsSetSettings` function.
 
-| Setting                 | Description                                                                                                                                                                                                                   |
-|-------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
-| `convention`            | Earth axes convention (NWD, ENU, or NED).                                                                                                                                                                                     |
-| `gain`                  | Determines the influence of the gyroscope relative to other sensors.  A value of zero will disable initialisation and the acceleration and magnetic rejection features.  A value of 0.5 is appropriate for most applications. |
-| `accelerationRejection` | Threshold (in degrees) used by the acceleration rejection feature.  A value of zero will disable this feature.  A value of 10 degrees is appropriate for most applications.                                                   |
-| `magneticRejection`     | Threshold (in degrees) used by the magnetic rejection feature.  A value of zero will disable the feature. A value of 10 degrees is appropriate for most applications.                                                         |
-| `recoveryTriggerPeriod` | Acceleration and magnetic recovery trigger period (in samples).  A value of zero will disable the acceleration and magnetic rejection features.  A period of 5 seconds is appropriate for most applications.                  |
+| Setting                 | Description                                                                                                                                                                                                                                                  |
+|-------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| `convention`            | Earth axes convention (NWD, ENU, or NED).                                                                                                                                                                                                                    |
+| `gain`                  | Determines the influence of the gyroscope relative to other sensors.  A value of zero will disable initialisation and the acceleration and magnetic rejection features.  A value of 0.5 is appropriate for most applications.                                |
+| `gyroscopeRange`        | Gyroscope range (in degrees pe second).  Angular rate recovery will activate if the gyroscope measurement exceeds 98% of this value.  A value of zero will disable this feature.  The value should be set to the range specified in the gyroscope datasheet. |
+| `accelerationRejection` | Threshold (in degrees) used by the acceleration rejection feature.  A value of zero will disable this feature.  A value of 10 degrees is appropriate for most applications.                                                                                  |
+| `magneticRejection`     | Threshold (in degrees) used by the magnetic rejection feature.  A value of zero will disable the feature. A value of 10 degrees is appropriate for most applications.                                                                                        |
+| `recoveryTriggerPeriod` | Acceleration and magnetic recovery trigger period (in samples).  A value of zero will disable the acceleration and magnetic rejection features.  A period of 5 seconds is appropriate for most applications.                                                 |
 
 ### Algorithm internal states
 
@@ -66,6 +71,7 @@ The AHRS algorithm flags are defined by the `FusionAhrsFlags` structure and obta
 | Flag                   | Description                                |
 |------------------------|--------------------------------------------|
 | `initialising`         | `true` if the algorithm is initialising.   |
+| `angularRateRecovery`  | `true` if angular rate recovery is active. |
 | `accelerationRecovery` | `true` if acceleration recovery is active. |
 | `magneticRecovery`     | `true` if a magnetic recovery is active.   |