Version 2.3.0

Extended Kalman filter added. A number of bug fixes.

Author: reefwing

LICENSE.md

@@ -14,20 +14,6 @@ The above copyright notice and this permission notice shall be included in all c
 
 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 
-/******************************************************************
-   GNU License applicable to Kalman Filter.
-   
- ******************************************************************/
-
-Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
-
-This software may be distributed and modified under the terms of the GNU
-General Public License version 2 (GPL2) as published by the Free Software
-Foundation and appearing in the file GPL2.TXT included in the packaging of
-this file. Please note that GPL2 Section 2[b] requires that all works based
-on this software must also be made publicly available under the terms of
-the GPL2 ("Copyleft").
-
 /******************************************************************
    GNU License applicable to code associated with Madgwick and
    Mahony Filters.

README.md

@@ -1,4 +1,4 @@
-![version](https://img.shields.io/github/v/tag/Reefwing-Software/Reefwing-AHRS) ![license](https://img.shields.io/badge/license-MIT-green) ![release](https://img.shields.io/github/release-date/Reefwing-Software/Reefwing-AHRS?color="red") ![open source](https://badgen.net/badge/open/source/blue?icon=github)
+[![arduino-library-badge](https://www.ardu-badge.com/badge/ReefwingAHRS.svg?)](https://www.ardu-badge.com/ReefwingAHRS) ![version](https://img.shields.io/github/v/tag/Reefwing-Software/Reefwing-AHRS) ![license](https://img.shields.io/badge/license-MIT-green) ![release](https://img.shields.io/github/release-date/Reefwing-Software/Reefwing-AHRS?color="red") ![open source](https://badgen.net/badge/open/source/blue?icon=github)
 
 # Reefwing AHRS
 
@@ -10,6 +10,8 @@ An Attitude and Heading Reference System (AHRS) takes information from the Inert
 
  Version 2.2.0 added support for the Nano 33 BLE Sense Rev. 2. Version 2 of the Nano 33 BLE Sense, replaces the LSM9DS1 9 axis IMU with a combination of two IMUs, the BMI270, a 6 axis gyro & accelerometer and the BMM150, a 3 axis magnetometer. In order to support the new hardware, it makes sense to separate the sensor processing from the sensor fusion algorithms. This library version also added the Kalman Filter Fusion option.
 
+ Version 2.3.0 added support for an Extended Kalman Filter and fixed a number of bugs.
+
  A complete description of this library is available in our Medium article: [Reefwing AHRS Arduino Library for Drones](https://reefwing.medium.com/reefwing-ahrs-arduino-library-for-drones-part-1-6d6457231764).
 
 ## Reefwing IMU Types
@@ -24,7 +26,7 @@ The libraries that use this definition header are:
 - [ReefwingMPU6x00](https://github.com/Reefwing-Software/MPU6x00)
 - [ReefwingMPU6050](https://github.com/Reefwing-Software/Reefwing-MPU6050)
 
-The Structs Defined in the IMU Types Library are:
+The Structs defined in the IMU Types Library are:
 
 - `EulerAngles`
 - `InertialMessage`
@@ -62,7 +64,7 @@ To use the Reefwing AHRS Library, you need to also install the Reefwing_imuTypes
 
  - Complementary Filter
  - Madgwick Filter
- - Kalman Filter
+ - Extended Kalman Filter
  - Mahony Filter
  - Classic Complementary Filter: Euler Angles are updated using trigonometry
  - NONE: Euler angles are calculated but no sensor fusion filter is used.
@@ -216,7 +218,7 @@ This sketch is configured to work with the `MADGWICK`, `MAHONY`, `CLASSIC`, `COM
 - **Madgwick Filter** - uses quaternions and combines a complementary filter approach with adaptive gain to fuse the sensor data.
 - **Mahony Filter** - uses quaternions, another riff on complementary fusion and includes an error correction step to compensate for gyroscope bias.
 - **Classic Complementary Filter** - Euler Angles are updated using trigonometry, apart from this it is similar to the Quaternion version.
-- **Kalman Filter** - can be used to minimise the impact of noise in sensors.
+- **Extended Kalman Filter** - can be used to minimise the impact of noise in sensors.
 - **NONE** - Euler angles are calculated but no sensor fusion is used. This is useful for demonstrating why you need sensor fusion!
 
 The loop frequency will vary with the board and fusion algorithm. The `COMPLEMENTARY` filter is a bit slower than the `MADGWICK` and `MAHONY`. We found that we experienced significant gyroscopic drift with the classic complementary filter. The Madgwick and Mahony filters fixes this issue but take a bit longer to settle on an angle. Of the two, Mahony is a bit faster than Madgwick, but the best filter and associated free parameter settings will depend on the application. The `NONE` option is also fast but has crazy gyroscopic drift.

examples/betaOptimisation/betaOptimisation.ino

@@ -14,9 +14,10 @@
   2.0.1 Invert Gyro Values PR                     24/12/22
   2.1.0 Updated Fusion Library                    30/12/22
   2.2.0 Add support for Nano 33 BLE Sense Rev. 2  10/02/23
+  2.3.0 Fixed RMS Error calculation               22/11/24
 
   This sketch attempts to determine the optimum beta for the
-  Madgwick filter. The Nano 33 BLE should be placed flat and
+  Madgwick filter. The Nano 33 BLE R1 should be placed flat and
   not moved for the duration of the test.
 
   The default sample rate for this library is 238 Hz for the 
@@ -44,17 +45,15 @@ float sampleArray[SAMPLES_PER_BETA];
 float err, optimumBeta, minError = INFINITY;
 int ctr = 0;           //  Track samples collected for each value of beta
 
-float rms(float arr[]) {
+float rms(float arr[], int n) {
     float square = 0.0, mean = 0.0;
-    int n = sizeof(arr) / sizeof(arr[0]);
- 
+
     // Calculate square.
     for (int i = 0; i < n; i++) {
         square += pow(arr[i], 2);
     }
- 
-    mean = (square / (float)(n));   // Calculate Mean.
- 
+
+    mean = square / (float)n;   // Calculate Mean.
     return sqrt(mean);
 }
 
@@ -63,10 +62,8 @@ void setup() {
   imu.begin();
   ahrs.begin();
 
-  //  Positive magnetic declination - Sydney, AUSTRALIA
-  ahrs.setDeclination(12.717);
   ahrs.setFusionAlgorithm(SensorFusion::MADGWICK);
-  ahrs.setBeta(beta);
+  ahrs.setDeclination(12.717); //  Sydney, AUSTRALIA
 
   //  Start Serial and wait for connection
   Serial.begin(115200);
@@ -108,15 +105,15 @@ void setup() {
       ahrs.setData(imu.data);
       ahrs.update();
 
-      //  load sampleArray with measured pitch and roll angles
+      //  load sampleArray with either pitch or roll angles
       sampleArray[ctr] = ahrs.angles.roll;
       ctr++;
-      sampleArray[ctr] = ahrs.angles.pitch;
-      ctr++;
+      // sampleArray[ctr] = ahrs.angles.pitch;
+      // ctr++;
       delay(10);  //  Wait for new sample
     }
     ctr = 0;
-    err = rms(sampleArray);
+    err = rms(sampleArray, SAMPLES_PER_BETA);
     if (err < minError) {
       minError = err;
       optimumBeta = beta;

examples/betaOptimisationR2/betaOptimisationR2.ino

@@ -0,0 +1,128 @@
+/******************************************************************
+  @file       betaOptimisationR2.ino
+  @brief      Determine optimal beta for Madgwick Filter
+  @author     David Such
+  @copyright  Please see the accompanying LICENSE file.
+
+  Code:        David Such
+  Version:     1.0.0
+  Date:        22/11/24
+
+  1.0.0 Original Release.                         22/11/24
+
+  This sketch targets the Nano 33 BLE Rev. 2 and attempts 
+  to determine the optimum beta for the Madgwick filter. 
+  The Nano 33 BLE should be placed flat and not moved for 
+  the duration of the test.
+
+  The default sample rate for this library is 100 Hz for the 
+  gyro/accelerometer and 10 Hz for the magnetometer. Thus a 
+  new gyro/acc reading should be available every 10 ms. We delay
+  for 15 ms between readings.
+
+******************************************************************/
+
+#include <ReefwingAHRS.h>
+#include <Arduino_BMI270_BMM150.h>
+
+ReefwingAHRS ahrs;
+SensorData data;
+
+#define MIN_BETA          0.0
+#define MAX_BETA          0.5
+#define BETA_INC          0.01
+#define SAMPLES_PER_BETA  20
+#define GYRO_MEAS_ERROR   M_PI * (40.0f / 180.0f)               // gyroscope measurement error in rads/s (default is 40 deg/s)
+#define DEFAULT_BETA      sqrt(3.0f / 4.0f) * GYRO_MEAS_ERROR
+
+float beta = MIN_BETA;   //  Madgwick filter gain, Beta
+float sampleArray[SAMPLES_PER_BETA];
+float err, optimumBeta, minError = INFINITY;
+int ctr = 0;           //  Track samples collected for each value of beta
+
+float rms(float arr[], int n) {
+    float square = 0.0, mean = 0.0;
+
+    // Calculate square.
+    for (int i = 0; i < n; i++) {
+        square += pow(arr[i], 2);
+    }
+
+    mean = square / (float)n;   // Calculate Mean.
+    return sqrt(mean);
+}
+
+void setup() {
+  //  Initialise the AHRS
+  ahrs.begin();
+
+  //  Positive magnetic declination - Sydney, AUSTRALIA
+  ahrs.setFusionAlgorithm(SensorFusion::MADGWICK);
+  ahrs.setDeclination(12.717);
+
+  //  Start Serial and wait for connection
+  Serial.begin(115200);
+  while (!Serial);
+
+  Serial.print("Detected Board - ");
+  Serial.println(ahrs.getBoardTypeString());
+
+  if (IMU.begin()) {
+    Serial.println("BMI270 & BMM150 IMUs Connected."); 
+
+    delay(20);
+    //  Flush the first reading - this is important!
+    //  Particularly after changing the configuration.
+    IMU.readGyroscope(data.gx, data.gy, data.gz);
+    IMU.readAcceleration(data.ax, data.ay, data.az);
+
+    //  Start processing IMU data.
+    Serial.println("Starting Beta Optimisation.\n"); 
+    Serial.println("Nano 33 BLE should remain motionless for duration of test.\n");
+    delay(1000); 
+    Serial.print("Beta");  Serial.println("\tRMS Error");
+  } 
+  else {
+    Serial.println("IMU Not Detected.");
+    while(1);
+  }
+
+  while (beta < MAX_BETA) {
+    while (ctr < SAMPLES_PER_BETA) {
+      //  Check for new IMU data and update angles
+      if (IMU.gyroscopeAvailable()) {  IMU.readGyroscope(data.gx, data.gy, data.gz);  }
+      if (IMU.accelerationAvailable()) {  IMU.readAcceleration(data.ax, data.ay, data.az);  }
+      if (IMU.magneticFieldAvailable()) {  IMU.readMagneticField(data.mx, data.my, data.mz);  }
+  
+      ahrs.setData(data);
+      ahrs.update();
+
+      //  load sampleArray with either pitch or roll angles
+      sampleArray[ctr] = ahrs.angles.roll;
+      ctr++;
+      // sampleArray[ctr] = ahrs.angles.pitch;
+      // ctr++;
+      delay(15);  //  Wait for new sample
+    }
+    ctr = 0;
+    err = rms(sampleArray, SAMPLES_PER_BETA);
+    if (err < minError) {
+      minError = err;
+      optimumBeta = beta;
+    }
+    
+    Serial.print(beta); Serial.print("\t"); Serial.println(err);
+    beta += BETA_INC;
+  }
+
+  Serial.print("\nOptimum Beta = ");
+  Serial.print(optimumBeta);
+  Serial.print(", with an RMS error of ");
+  Serial.print(minError);
+  Serial.println(" degrees.");
+  Serial.print("Default Beta = ");
+  Serial.println(DEFAULT_BETA);
+  Serial.println("\nTEST CONCLUDED");
+}
+
+void loop() { }

examples/filterTest/filterTest.ino

@@ -0,0 +1,138 @@
+/******************************************************************
+  @file       filter_test.ino
+  @brief      The sketch simulates IMU data with and without noise, 
+              evaluates singularities, and measures accuracy and drift.
+              
+  @author     David Such
+  @copyright  Please see the accompanying LICENSE file.
+
+  Code:        David Such
+  Version:     1.0.0
+  Date:        23/11/24
+
+  1.0.0 Original Release.                         23/11/24
+
+******************************************************************/
+
+#include <ReefwingAHRS.h>
+
+// Select the simulation parameters
+#define SIMULATION_DURATION 20.0f // Seconds
+#define SIMULATION_STEP 0.01f     // Seconds (100 Hz sampling rate)
+#define NOISE_STDDEV 0.02f        // Standard deviation of noise for acceleration and gyro (rad/s and g)
+#define ACTUAL_YAW 30.0f          // Ground truth yaw (degrees) - fixed
+#define MAX_ROLL  30.0f           // Limit of synthesized roll angle ±MAX_ROLL (degrees)
+#define MAX_PITCH 20.0f           // Limit of synthesized pitch angle ±MAX_PITCH (degrees)
+
+// Instantiate AHRS
+ReefwingAHRS ahrs;
+SensorData simulatedData;
+
+// Timing variables
+unsigned long startTime;
+float currentTime;
+
+// Output variables
+float rollError, pitchError, yawError;
+
+// Function to add Gaussian noise
+float addNoise(float value, float stddev) {
+  return value + (stddev * ((float)random(-1000, 1000) / 1000.0f)); // Simulate noise
+}
+
+// Function to simulate ideal IMU data
+void simulateIMUData(float t, bool addNoiseFlag, float &actualRoll, float &actualPitch, float &actualYaw) {
+  // Ground truth: sinusoidal roll, pitch; constant yaw
+  actualRoll = MAX_ROLL * sin(2 * PI * 0.1f * t);   // Roll in degrees
+  actualPitch = MAX_PITCH * cos(2 * PI * 0.1f * t); // Pitch in degrees
+  actualYaw = ACTUAL_YAW;                           // Yaw in degrees
+
+  // Simulate gyro data (rad/s)
+  simulatedData.gx = radians(2 * PI * 0.1f * MAX_ROLL * cos(2 * PI * 0.1f * t));   // d(roll)/dt
+  simulatedData.gy = radians(-2 * PI * 0.1f * MAX_PITCH * sin(2 * PI * 0.1f * t)); // d(pitch)/dt
+  simulatedData.gz = 0.0f; // d(yaw)/dt
+
+  // Simulate accelerometer data (g)
+  simulatedData.ax = sin(radians(actualPitch));
+  simulatedData.ay = -sin(radians(actualRoll)) * cos(radians(actualPitch));
+  simulatedData.az = cos(radians(actualRoll)) * cos(radians(actualPitch));
+
+  // Simulate magnetometer data (arbitrary units)
+  simulatedData.mx = cos(radians(actualYaw)) * cos(radians(actualPitch));
+  simulatedData.my = sin(radians(actualYaw)) * cos(radians(actualPitch));
+  simulatedData.mz = -sin(radians(actualPitch));
+
+  // Add noise if required
+  if (addNoiseFlag) {
+    simulatedData.gx = addNoise(simulatedData.gx, NOISE_STDDEV);
+    simulatedData.gy = addNoise(simulatedData.gy, NOISE_STDDEV);
+    simulatedData.gz = addNoise(simulatedData.gz, NOISE_STDDEV);
+
+    simulatedData.ax = addNoise(simulatedData.ax, NOISE_STDDEV);
+    simulatedData.ay = addNoise(simulatedData.ay, NOISE_STDDEV);
+    simulatedData.az = addNoise(simulatedData.az, NOISE_STDDEV);
+
+    simulatedData.mx = addNoise(simulatedData.mx, NOISE_STDDEV);
+    simulatedData.my = addNoise(simulatedData.my, NOISE_STDDEV);
+    simulatedData.mz = addNoise(simulatedData.mz, NOISE_STDDEV);
+  }
+
+  // Set the simulated data in the AHRS library
+  ahrs.setData(simulatedData);
+}
+
+// Test a single filter
+void testFilter(SensorFusion filter, bool addNoiseFlag) {
+  ahrs.reset();
+  ahrs.setFusionAlgorithm(filter);
+  ahrs.setDOF(DOF::DOF_9);
+
+  Serial.print("Testing filter: ");
+  Serial.print((int)filter);
+  Serial.println(addNoiseFlag ? " with noise" : " without noise");
+  Serial.println("Time,Roll Error,Pitch Error,Yaw Error");
+
+  currentTime = 0.0f;
+  startTime = millis();
+
+  while (currentTime < SIMULATION_DURATION) {
+    float actualRoll, actualPitch, actualYaw;
+
+    simulateIMUData(currentTime, addNoiseFlag, actualRoll, actualPitch, actualYaw);
+    ahrs.update();
+
+    // Calculate errors using ground-truth angles
+    rollError = ahrs.angles.roll - actualRoll;   // Error in roll angle
+    pitchError = ahrs.angles.pitch - actualPitch; // Error in pitch angle
+    yawError = ahrs.angles.yaw - actualYaw;       // Error in yaw angle
+
+    // Output results
+    Serial.print(currentTime, 3);
+    Serial.print(",");
+    Serial.print(rollError, 3);
+    Serial.print(",");
+    Serial.print(pitchError, 3);
+    Serial.print(",");
+    Serial.println(yawError, 3);
+
+    currentTime += SIMULATION_STEP;
+    delay(SIMULATION_STEP * 1000);
+  }
+}
+
+void setup() {
+  // Start Serial and wait for connection
+  Serial.begin(115200);
+  while (!Serial);
+
+  // Initialize AHRS
+  ahrs.begin();
+
+  // Test each filter
+  for (int i = 0; i <= (int)SensorFusion::NONE; i++) {
+    testFilter(static_cast<SensorFusion>(i), false); // Test without noise
+    testFilter(static_cast<SensorFusion>(i), true);  // Test with noise
+  }
+}
+
+void loop() { }