Arduino EKF implementation

[Juaxs2022]

Hello devs, thank you for your work.

Ive been using an old version of your filter (uNavINS) in arduino for a while and i just want to start using this version of the code. The problem I have is that I havent been able to make it work using Arduino platform, maybe I doing a wrong implementation of the funtions.

I was wonderinng if theres any Arduino implementation of this code for the Arduino enviroment. Im using a Teensy 4.1, ublox 8M and WT901 IMU.

Thank you.

[flybrianfly]

Unfortunately, we do not have an Arduino version of this library implemented yet.

[mjs513]

@flybrianfly
Since its pretty quiet I am in the process of seeing if I can get your latest and greatest EKF15 working on the Teensy. I did find that cout and setprecision works on the teensy now so that was an easy one to fix but I am getting stuck on a few other things mainly due to thinks like:

error: 'navigation' has not been declared
   49 |   navigation::Ekf15State ekf;
error: 'types' has not been declared
   50 |   types::Imu imu;
error: 'nav' has not been declared
   65 |   cout << nav::constants::ECC2 << std::endl;

The only namespace I see used is bfs in the source or am I missing something

[mjs513]

Hmm thought I wrote something but not showing probably forgot to hit comment. Any got it runnning but commented out types::IMU imu and ekf.update and got this:

l0:
0.61
-1.86
2006.48

l1:
35.22889491773
-106.68602803228
1981.6934422879

lla2: 
35.679862
-105.962417
4000.0000000002

ecc2: 
0.00669437999014

ypr
 1.3962634015955
0.34906585039887
0.17453292519943

eul
 1.3962634015955
0.34906585039887
0.17453292519943

eul2:
 1.3962634015955
0.34906585039887
0.17453292519943

eul3:
 1.3962634015955
0.34906585039887
0.17453292519943

q:
0.76126399009278
-0.045443294018814
0.18768755374174
0.61901973036986

q2:
0.76126399009278
-0.045443294018814
0.18768755374174
0.61901973036986

q3:
0.76126399009278
-0.045443294018814
0.18768755374174
0.61901973036986

Going to assume a few things with this next commet. In setup I will need to do a:
ekf.intialize
then in loop do TimeUpdate on gyro and accel updates and a MeasuremenUpdate when I have GPS data? Not sure that is the correct process - it is a bit different than the uNAVins setup.

[flybrianfly]

@mjs513 are you using the latest version downloaded from GitHub? I don't see a types.h include and there shouldn't be one.

[flybrianfly]

@mjs513 the assumptions are correct; however, you need good GPS data to call ekf.initialize, so I usually use a boolean flag in loop to trigger initialization. Something like the following pseudo code:

bool ekf_initialized = false;
bool new_imu_data, new_gps_data;

void setup() {
  // setup IMU and GPS
}

void loop() {
  new_imu_data = imu.Read();
  if (new_imu_data) {
    // get the IMU data
  }
  new_gps_data = gps.Read();
  if (new_gps_data) {
    // get the GPS data
  }
  if (!ekf_initialized) {
    if (new_imu_data && new_gps_data && (gps.Fix >=3) && (gps.num_sats >=7)) {
      ekf.initialize();
      ekf_initialized = true;
    } 
  } else {
    if (new_imu_data) {
      ekf.TimeUpdate();
    }
    if (new_gps_data) {
      ekf.MeasurementUpdate();
    }
}

[mjs513]

Hi Brian
I remember that when we were doing the uNavINS days on the Teensy forum. Think I also added a requirement for num satellites and pdop or hdop - forgot which :).

Going to play some more and see how it goes over the next couple of days. Maybe better I put that on the teensy forum :)

[mjs513]

@mjs513 are you using the latest version downloaded from GitHub? I don't see a types.h include and there shouldn't be one.

Oops missed this comment - its in the arduino and cmake examples - its currently commented out

[flybrianfly]

@mjs513, thanks. The examples are a mess that I should probably clean up. I keep wanting to create a types library (i.e. an IMU type, a pressure type, airspeed type, etc) to improve type safety over plain old floats and make it so it's easy to ignore units, but that's a rabbit hole I haven't found a satisfying solution to yet.

[mjs513]

Good Morning @flybrianfly

Sorry for the delay but I put together a sketch using a MPU9250 and a ardusimple F9P, not sure its 100% correct.

#include "Streaming.h"
#define cout Serial

elapsedMillis output;

/******* Setup Ublox F9P receiver *********/
#include "ubx.h"
/* Ublox object, GNSS on Serial3 */
bfs::Ubx gps(&Serial1);

/******* Setup MPU9250 *********/
#include "mpu9250.h"
/* Mpu9250 object */
bfs::Mpu9250 imu;

using namespace bfs;

#include "navigation.h"
Ekf15State ekf;

bool ekf_initialized = false;
bool new_imu_data, new_gps_data;
float accel_[3], gyro_[3], mag_[3];
float dt;
long t_old;

Eigen::Vector3f accel, gyro, mag;
Eigen::Vector3f velNed;
Eigen::Vector3d llh;

void setup() {
  Serial.begin(115200);
  
  delay(2000);
  if (!gps.AutoBegin()) {
    Serial.println("Unable to establish communication and configure GNSS RX");
    while (1) {}
  }

  /* Start the I2C bus */
  Wire.begin();
  Wire.setClock(400000);
  /* I2C bus,  0x68 address */
  imu.Config(&Wire, bfs::Mpu9250::I2C_ADDR_PRIM);
  /* Initialize and configure IMU */
  if (!imu.Begin()) {
    Serial.println("Error initializing communication with IMU");
    while(1) {}
  }
  /* Set the sample rate divider */
  if (!imu.ConfigSrd(19)) {
    Serial.println("Error configured SRD");
    while(1) {}
  }

}

void loop() {
  new_imu_data = imu.Read(accel_, gyro_, mag_);
  if (new_imu_data) {
    dt = (millis() - t_old) / 1000.0f;
    t_old = millis();
    // get the IMU data
    accel = { accel_[0], accel_[1], accel_[2]};
    gyro = { gyro_[0], gyro_[1], gyro_[2]};
    mag = { mag_[0], mag_[1], mag_[2]};
  }
  new_gps_data = gps.Read();
  if (new_gps_data) {
    // get the GPS data
    llh = gps.llh_rad_m();
    velNed = gps.ned_vel_mps();
  }

  if (!ekf_initialized) {
    if (new_imu_data && new_gps_data && (gps.fix() <=3) && (gps.num_sv() >=7)) {
      ekf.Initialize(accel, gyro, mag, velNed, llh);
      ekf_initialized = true;
    } 
  } else {
    if (new_imu_data) {
      ekf.TimeUpdate(accel, gyro, dt);
    }
    if (new_gps_data) {
      ekf.MeasurementUpdate(velNed, llh);
    }
  }
  
  if(output > 1000) {
    cout << ekf.yaw_rad()  * 180./M_PI << ", " << ekf.pitch_rad() * 180./M_PI << ", ";
    cout << ekf.roll_rad() * 180./M_PI << ", " << gps.pdop() << endl;
    output = 0;
  }

}


// PRINT MATRIX (float type)
// By: randomvibe
//-----------------------------
void print_mtxf(const Eigen::Vector3f& X)  
{
   int j, ncol;
   ncol = 3;
   //Serial.print("ncol: "); Serial.println(ncol);       
   //Serial.println();
    for (j=0; j<ncol; j++)
    {
        Serial.print(X(j), 6);   // print 6 decimal places
        Serial.print(", ");
    }
    Serial.println();
}

void print_mtxd(const Eigen::Vector3d& X)  
{
   int j, ncol;
   ncol = 3;
   //Serial.print("ncol: "); Serial.println(ncol);       
   //Serial.println();
    for (j=0; j<ncol; j++)
    {
        Serial.print(X(j), 6);   // print 6 decimal places
        Serial.print(", ");
    }
    Serial.println();
}

[mjs513]

Part 2 - yaw when just sitting on the desk is an issue as well after I so some movements, only pitch looks ok but roll seems to went crazy along with yaw - note I didn't roll the imu that much.

[flybrianfly]

@mjs513, I just ran this code and it's working as expected:

#include "mpu9250.h"
#include "ubx.h"
#include "navigation.h"

/* MPU9250 IMU on SPI with CS pin 10 */
bfs::Mpu9250 imu(&SPI, 36);

/* Sample rate divider for 100 Hz update rate */
static constexpr int8_t IMU_SRD = 9;

/* Digital low pass filter */
static constexpr bfs::Mpu9250::DlpfBandwidth DLPF = bfs::Mpu9250::DLPF_BANDWIDTH_10HZ;

/*
* UBLOX GNSS on UART2 with the following packets configured:
*   * UBX-NAV-DOP
*   * UBX-NAV-EOE
*   * UBX-NAV-POSECEF
*   * UBX-NAV-PVT
*   * UBX-NAV-VELECEF
*   * UBX-NAV-TIMEGPS
*   * UBX-NAV-HPPOSECEF
*   * UBX-NAV-HPPOSLLH
*
* Baudrate set to 921600 and a 10 Hz navigation update rate. Dynamic model set
* to airborne 4g.
*/
bfs::Ubx gnss(&Serial3);
static constexpr int32_t GNSS_BAUD = 921600;

/* EKF settings */
bool ekf_initialized = false;
bool new_mag_data, new_gnss_data;
static constexpr float DT = 1.0f / 100.0f;  // 100 Hz IMU rate
Eigen::Vector3f accel_mps2, gyro_radps, mag_ut, ned_vel_mps;
Eigen::Vector3d llh;
bfs::Ekf15State ekf;

/* Function to print error message and halt */
void Error(String string) {
  Serial.print("ERROR: ");
  Serial.println(string);
  while (1) {}
}

void setup() {
  Serial.begin(115200);
  while (!Serial) {}
  Serial.println("Starting EKF test");
  SPI.begin();
  /* Init and configure IMU */
  if (!imu.Begin()) {
    Error("Unable to initialize communication with IMU");
  }
  if (!imu.ConfigAccelRange(bfs::Mpu9250::ACCEL_RANGE_16G)) {
    Error("Unable to configure IMU accel range");
  }
  if (!imu.ConfigGyroRange(bfs::Mpu9250::GYRO_RANGE_2000DPS)) {
    Error("Unable to configure IMU gyro range");
  }
  if (!imu.ConfigSrd(IMU_SRD)) {
    Error("Unable to configure IMU sample rate divider");
  }
  if (!imu.ConfigDlpfBandwidth(DLPF)) {
    Error("Unable to configure IMU DLPF");
  }
  /* Init and configure GNSS */
  if (!gnss.Begin(GNSS_BAUD)) {
    Error("Unable to initialize communication with GNSS");
  }
  Serial.println("Setup complete");
}

void loop() {
  /* Check for new GNSS data */
  if (gnss.Read()) {
    new_gnss_data = true;
    llh[0] = gnss.lat_rad();
    llh[1] = gnss.lon_rad();
    llh[2] = gnss.alt_wgs84_m();
    ned_vel_mps[0] = gnss.north_vel_mps();
    ned_vel_mps[1] = gnss.east_vel_mps();
    ned_vel_mps[2] = gnss.down_vel_mps();
  }
  /* Check for new IMU data and run EKF */
  if (imu.Read()) {
    accel_mps2[0] = imu.accel_x_mps2();
    accel_mps2[1] = imu.accel_y_mps2();
    accel_mps2[2] = imu.accel_z_mps2();
    gyro_radps[0] = imu.gyro_x_radps();
    gyro_radps[1] = imu.gyro_y_radps();
    gyro_radps[2] = imu.gyro_z_radps();
    new_mag_data = imu.new_mag_data();
    if (new_mag_data) {
      mag_ut[0] = imu.mag_x_ut();
      mag_ut[1] = imu.mag_y_ut();
      mag_ut[2] = imu.mag_z_ut();
    }
    /* Initialize and run the EKF */
    if (!ekf_initialized) {
      if (new_mag_data && new_gnss_data && (gnss.fix() >= 3) &&
          (gnss.num_sv() > 7)) {
        ekf.Initialize(accel_mps2, gyro_radps, mag_ut, ned_vel_mps, llh);
        ekf_initialized = true;
      }
    } else {
      /* Time update occurs every frame since we're tied to IMU read */
      ekf.TimeUpdate(accel_mps2, gyro_radps, DT);
      /* Measurement update if new GNSS data */
      if (new_gnss_data) {
        ekf.MeasurementUpdate(ned_vel_mps, llh);
      }
    }
    /* Reset the new GNSS data flag */
    if (new_gnss_data) {
      new_gnss_data = false;
    }
    /* Output Euler angles */
    Serial.print(bfs::rad2deg(ekf.pitch_rad()));
    Serial.print("\t");
    Serial.print(bfs::rad2deg(ekf.roll_rad()));
    Serial.print("\t");
    Serial.print(bfs::rad2deg(ekf.yaw_rad()));
    Serial.print("\n");


  }
}

Uses the latest versions of the InvenSense IMU and uBlox libraries downloaded directly from GitHub. Running on a Teensy 4.1.

I would expect that yaw might drift some if everything is stationary. The EKF won't be able to track heading well without dynamic motion - the rule of thumb is that yaw is generally accurate when inertial speed is above about 5 m/s. I would expect pitch and roll to be accurate though.

[mjs513]

@flybrianfly
Getting back to this after getting severely distracted on a couple of other things. Anyway wanted to ask what GPS unit are you using.

[flybrianfly]

@mjs513, I typically use either a SAM-M8Q or a ZED-F9P

[mjs513]

@flybrianfly, that was fast thanks - I do have a F9P (ardusimple version) may give that a try - just have to figure out the setting screen :)

Oh by I added support for the ICM20649 (high-g) sensor (no mag though) in my branch if you are interested

[flybrianfly]

@mjs513, that would be a good add to the InvenSense IMU library. I have one sitting around that I haven't had a chance to play with yet.

[brightproject]

I just ran this code and it's working as expected:

Good day @mjs513 and @flybrianfly
I tried to compile sample code for the stm32f411 microcontroller and encountered the following problems:

1. The compiler cursed

ekf_15_state.h:287:58: error: expected primary-expression before 'float'
   287 | y_.segment(0, 3) = lla2ned(lla, ins_lla_rad_m_).cast<float>();

I had to do it in two lines

navigation/src/ekf_15_state.h

Line 227 in 9029646

fs_(5, 2) = -2.0f * G_MPS2<float> / SEMI_MAJOR_AXIS_LENGTH_M;

and here

navigation/src/ekf_15_state.h

Line 405 in 9029646

(Eigen::Vector3f() << 0.0f, 0.0f, G_MPS2<float>).finished();

Replace G_MPS2<float> by G_MPS2
2. Also a compilation error

error: expected primary-expression before ';' token
    83 | ang += BFS_2PI<T>;

There is a separate quest, in the units library, it says to use such constants
https://github.com/bolderflight/units?tab=readme-ov-file#constants
and in the library navigation this generally refers to utilities
https://github.com/bolderflight/navigation?tab=readme-ov-file#utilities
As a result, I changed BFS_PI to WrapToPi and BFS_2PI to WrapTo2Pi in file.
The source code of the sketch is below, maybe someone will collect it like me for stm32.

#define PROCESSING
// #define DEBUG

#define STATUS_LED PC13

// represents a safer and preferred way to define constants in modern C++ because they provide type safety and scope. 
// while #define does not have these properties and may lead to unwanted effects due to its textual nature.
// static constexpr int32_t GNSS_BAUD = 921600;


// #include "mpu9250.h"

#include <LSM6DS3_Seeed.h> // Accel + Gyro
#include <MechaQMC5883.h> // Mag
#include <ubx.h> // Neo-m8n
#include <navigation.h> // Navigation filters

#define GNSS_BAUD 9600
#define SERIAL_BAUD 115200 // max data rate 100 Hz
// #define SERIAL_BAUD 230400 // max data rate 200 Hz
// #define SERIAL_BAUD 921600 // max data rate 1000 Hz

#define G 9.80665;
#define DEG_TO_RAD 180/M_PI;
#define RAD_TO_DEG M_PI/180;

#define GYRO_RANGE 2000.0f
#define GYRO_SCALE 28571.4f

#define ACCEL_RANGE 8.0f
#define ACCEL_SCALE 32768.0f

#define DELAY 350

// feature Arduino_Core_STM32
HardwareSerial Serial2(USART2); // PA2(TX2), PA3(RX2)

// class for GPS ubx protocol
//  Baudrate set to 921600 and a 10 Hz navigation update rate.
// Dynamic model set to airborne 4g.
bfs::Ubx gnss(&Serial2);

//Create a instance of class LSM6DS3
LSM6DS3 imu(I2C_MODE, 0x6B);

//Create a instance of class QMC5883
MechaQMC5883 qmc;

/* MPU9250 IMU on SPI with CS pin 10 */
// bfs::Mpu9250 imu(&SPI, 36);

/* Sample rate divider for 100 Hz update rate */
// static constexpr int8_t IMU_SRD = 9;

/* Digital low pass filter */
// static constexpr bfs::Mpu9250::DlpfBandwidth DLPF = bfs::Mpu9250::DLPF_BANDWIDTH_10HZ;

/*
* UBLOX GNSS on UART2 with the following packets configured:
*   * UBX-NAV-DOP
*   * UBX-NAV-EOE
*   * UBX-NAV-POSECEF
*   * UBX-NAV-PVT
*   * UBX-NAV-VELECEF
*   * UBX-NAV-TIMEGPS
*   * UBX-NAV-HPPOSECEF
*   * UBX-NAV-HPPOSLLH
*

/* EKF settings */
bool ekf_initialized = false;
bool new_mag_data, new_gnss_data;
static constexpr float DT = 1.0f / 100.0f;  // 100 Hz IMU rate

float Gx, Gy, Gz, Ax, Ay, Az;
float GxRad, GyRad, GzRad, AxG, AyG, AzG;
int x, y, z, Mx, My, Mz;

const long displayPeriod = 5;  // 100 Hz IMU rate
unsigned long previousMillis = 0;

Eigen::Vector3f accel_mps2, gyro_radps, mag_ut, ned_vel_mps;
Eigen::Vector3d llh;
bfs::Ekf15State ekf;

//          Gyro + Accel LSM6DS3
//
//                  | Z axis
//                  |
//                  |   / Y axis
//              ____|__/____
//            / *   | /    /|       X axis
//           /      |/____//________
//          /___________ //
//         |____________|/

//             Mag QMC5883L
//
//                  | Z axis
//                  |
//                  |   / Y axis
//              ____|__/____
//            /     | /    /|       X axis
//           /      |/____//________
//          /_*_________ //
//         |____________|/
//
//

//   direction Axis  Ax, Ay, Az,Gx, Gy, Gz,Mx, My, Mz
int SENSOR_SIGN[9] = {1, -1, -1, 1, -1, -1, 1, -1, -1};

/* Function to print error message and halt */
void Error(String string) {
  Serial.print("ERROR: ");
  Serial.println(string);
  while (1) {}
}

void setup() {
    Serial.begin(SERIAL_BAUD);
    while (!Serial) {}
    #ifdef DEBUG
    Serial.println("Starting EKF test");
    #endif

    pinMode(STATUS_LED, OUTPUT);  // Status LED
    digitalWrite(STATUS_LED, HIGH); // stm32f411 led OFF
    
    // set LSM6DS3 parameters
    initLSM6DS3();

    //Call .begin() to configure the IMUs
    if (imu.begin() != 0) {
    #ifdef DEBUG
        Error("Device LSM6DS3 error");
    #endif
        digitalWrite(STATUS_LED, LOW); // stm32f411 led ON
        delay(DELAY);
        NVIC_SystemReset();
    } else {
    #ifdef DEBUG
        Serial.println("Device LSM6DS3 OK!");
    #endif
    }

    // init and set QMC5883 parameters
    QMCinit();

/* Init and configure IMU */
//   if (!imu.Begin()) {
//     Error("Unable to initialize communication with IMU");
//   }
//   if (!imu.ConfigAccelRange(bfs::Mpu9250::ACCEL_RANGE_16G)) {
//     Error("Unable to configure IMU accel range");
//   }
//   if (!imu.ConfigGyroRange(bfs::Mpu9250::GYRO_RANGE_2000DPS)) {
//     Error("Unable to configure IMU gyro range");
//   }
//   if (!imu.ConfigSrd(IMU_SRD)) {
//     Error("Unable to configure IMU sample rate divider");
//   }
//   if (!imu.ConfigDlpfBandwidth(DLPF)) {
//     Error("Unable to configure IMU DLPF");
//   }
  /* Init and configure GNSS */
  if (!gnss.Begin(GNSS_BAUD)) {
    #ifdef DEBUG
    Error("Unable to initialize communication with GNSS");
    #endif
  }
  #ifdef DEBUG
  Serial.println("Setup complete");
  #endif
}

void loop() {
  /* Check for new GNSS data */
  if (gnss.Read()) {
    new_gnss_data = true;
    llh[0] = gnss.lat_rad();
    llh[1] = gnss.lon_rad();
    llh[2] = gnss.alt_wgs84_m();
    ned_vel_mps[0] = gnss.north_vel_mps();
    ned_vel_mps[1] = gnss.east_vel_mps();
    ned_vel_mps[2] = gnss.down_vel_mps();
  }
  /* Check for new IMU data and run EKF */
//   if (imu.Read()) {

if (millis() - previousMillis >= displayPeriod)
{
    previousMillis = millis();

    digitalWrite(STATUS_LED, !digitalRead(STATUS_LED)); 

    // accel_mps2[0] = imu.accel_x_mps2();
    // accel_mps2[1] = imu.accel_y_mps2();
    // accel_mps2[2] = imu.accel_z_mps2();

    accel_mps2[0] = SENSOR_SIGN[0]*imu.readRawAccelX() * ACCEL_RANGE / ACCEL_SCALE;
    accel_mps2[0] *= G; // in 9.8 m/s/s
    accel_mps2[1] = SENSOR_SIGN[1]*imu.readRawAccelY() * ACCEL_RANGE / ACCEL_SCALE;
    accel_mps2[1] *= G; // in 9.8G m/s/s
    accel_mps2[2] = SENSOR_SIGN[2]*imu.readRawAccelZ() * ACCEL_RANGE / ACCEL_SCALE;
    accel_mps2[2] *= G; // in 9.8G m/s/s

    // gyro_radps[0] = imu.gyro_x_radps();
    // gyro_radps[1] = imu.gyro_y_radps();
    // gyro_radps[2] = imu.gyro_z_radps();

    gyro_radps[0] = SENSOR_SIGN[3]*imu.readRawGyroX() * GYRO_RANGE / GYRO_SCALE;
    gyro_radps[0] *=  RAD_TO_DEG; // radians per second
    gyro_radps[1] = SENSOR_SIGN[4]*imu.readRawGyroY() * GYRO_RANGE / GYRO_SCALE;
    gyro_radps[1] *=  RAD_TO_DEG; // radians per second
    gyro_radps[2] = SENSOR_SIGN[5]*imu.readRawGyroZ() * GYRO_RANGE / GYRO_SCALE;
    gyro_radps[2] *=  RAD_TO_DEG; // radians per second

    // new_mag_data = imu.new_mag_data();
    new_mag_data = qmc.read(&x,&y,&z);
    if (new_mag_data) {
    //   mag_ut[0] = imu.mag_x_ut();
    //   mag_ut[1] = imu.mag_y_ut();
    //   mag_ut[2] = imu.mag_z_ut();

      mag_ut[0] = SENSOR_SIGN[6]*x;
      mag_ut[1] = SENSOR_SIGN[7]*y;
      mag_ut[2] = SENSOR_SIGN[8]*z;
    }
    /* Initialize and run the EKF */
    if (!ekf_initialized) {
      if (new_mag_data && new_gnss_data && (gnss.fix() >= 3) &&
          (gnss.num_sv() > 7)) {
        ekf.Initialize(accel_mps2, gyro_radps, mag_ut, ned_vel_mps, llh);
        ekf_initialized = true;
      }
    } else {
      /* Time update occurs every frame since we're tied to IMU read */
      ekf.TimeUpdate(accel_mps2, gyro_radps, DT);
      /* Measurement update if new GNSS data */
      if (new_gnss_data) {
        ekf.MeasurementUpdate(ned_vel_mps, llh);
      }
    }
    /* Reset the new GNSS data flag */
    if (new_gnss_data) {
      new_gnss_data = false;
    }

    #ifdef PROCESSING
    /* Output Euler angles */
    Serial.print("Orientation: ");
    Serial.print(bfs::rad2deg(ekf.pitch_rad()));
    Serial.print(" ");
    Serial.print(bfs::rad2deg(ekf.roll_rad()));
    Serial.print(" ");
    Serial.print(bfs::rad2deg(ekf.yaw_rad()));
    #endif


  }
}

void QMCinit(){

  qmc.init();
  qmc.setMode(Mode_Continuous,ODR_50Hz,RNG_8G,OSR_64);

}

void initLSM6DS3() {

    // myIMU.settings.gyroEnabled = 1;  //Can be 0 or 1
    imu.settings.gyroRange = GYRO_RANGE;   //Max deg/s.  Can be: 125, 245, 500, 1000, 2000
    imu.settings.gyroSampleRate = 416;   //Hz.  Can be: 13, 26, 52, 104, 208, 416, 833, 1666
    imu.settings.gyroBandWidth = 100;  //Hz.  Can be: 50, 100, 200, 400;
    // myIMU.settings.gyroFifoEnabled = 1;  //Set to include gyro in FIFO
    // myIMU.settings.gyroFifoDecimation = 1;  //set 1 for on /1

    // myIMU.settings.accelEnabled = 1;
    // myIMU.settings.accelODROff = 1;
    imu.settings.accelRange = ACCEL_RANGE;      //Max G force readable.  Can be: 2, 4, 8, 16
    imu.settings.accelSampleRate = 26;  //Hz.  Can be: 13, 26, 52, 104, 208, 416, 833, 1666, 3332, 6664, 13330
    imu.settings.accelBandWidth = 50;  //Hz.  Can be: 50, 100, 200, 400;
    // myIMU.settings.accelFifoEnabled = 1;  //Set to include accelerometer in the FIFO
    // myIMU.settings.accelFifoDecimation = 1;  //set 1 for on /1
    // myIMU.settings.tempSensitivity = 2;

}

I am using LSM6DS3(gyroscope and accelerometer) and QMC5883(magnetometer) sensors.
I read the raw data and scale it:

1. Gyroscope in rad/sec
2. Accelerometer in m/s^2
3. Magnetometer in uT

Examplecode compiled.

The only thing I don’t understand is which library loads the SPI?
I don’t need it, all connections are via I2C and I would like to disable SPI compilation so that the firmware code takes up less space.
I use processing to visualize the orientation angles produced by the filter.
Visualizer.zip

And change the direction of the axes by changing the sign

//   direction Axis  Ax, Ay, Az,Gx, Gy, Gz,Mx, My, Mz
int SENSOR_SIGN[9] = {1, -1, 1, 1, -1, -1, 1, -1, -1};

My board with sensors is located like this, and the axes are as indicated in the code.

Please tell me about the GNSS antenna and receiver, should it be oriented according to the NED or ECEF convention or some other?

GNSS ANTENNA or RECEIVER
 
         Z axis
           |   / Y axis
     _____ |__/_____
    O      | /     O
   /       |/     /      X axis
  /    uBlox ___ /________
 /              /
O_____________O/

I would like examples of setting parameters

navigation/src/ekf_15_state.h

Line 331 in 9029646

* Sensor characteristics - accel and gyro are modeled with a

as well as sharing experience in compensating for temperature drifts of gyroscopes.

For me, drift due to negative temperatures became a problem on the L3GD20 gyroscope.
I wrote a simple interpolation function and it works for linear drift relationships.

float interpolate(float x1, float y1, float x2, float y2, float x) {
    return y1 + (x - x1) * (y2 - y1) / (x2 - x1);
}

void calculateGyroValues(float temperature, float& Gx, float& Gy, float& Gz, int16_t Gx_meas, int16_t Gy_meas, int16_t Gz_meas) {
if (temperature >= -10.0f && temperature <= 30.0f) {

    #ifdef DEBUG
    Serial.println("Temperature in Range -10 +30!");
    Serial.println(temperature);
    #endif

    // if (temperature == -10.0f) {
    //     Gx -= -32;
    //     Gy -= 15;
    //     Gz -= 54;
    // } 
    // if (temperature == 30.0) {
    //     Gx -= -2;
    //     Gy -= -12;
    //     Gz -= -10;
    // } 

    float x1 = -10;
    float y1_Gx = -32;
    float y1_Gy = 15;
    float y1_Gz = 54;

    float x2 = 30;
    float y2_Gx = -2;
    float y2_Gy = -12;
    float y2_Gz = -10;

    if(!flagtemp){

    Gx = Gx_meas - interpolate(x1, y1_Gx, x2, y2_Gx, temperature);
    Gy = Gy_meas - interpolate(x1, y1_Gy, x2, y2_Gy, temperature);
    Gz = Gz_meas - interpolate(x1, y1_Gz, x2, y2_Gz, temperature);

    #ifdef DEBUG
    Serial.println("Interpolate is normally!"); 
    #endif

    } else {

    // Gx = Gx_meas + interpolate(x1, y1_Gx, x2, y2_Gx, temperature);
    // Gy = Gy_meas + interpolate(x1, y1_Gy, x2, y2_Gy, temperature);
    // Gz = Gz_meas + interpolate(x1, y1_Gz, x2, y2_Gz, temperature);

    Gx = -Gx_meas;
    Gy = -Gy_meas;
    Gz = -Gz_meas;

    // Gx = -Gx_meas - interpolate(x1, y1_Gx, x2, y2_Gx, temperature);
    // Gy = -Gy_meas - interpolate(x1, y1_Gy, x2, y2_Gy, temperature);
    // Gz = -Gz_meas - interpolate(x1, y1_Gz, x2, y2_Gz, temperature);

    #ifdef DEBUG
    Serial.println("Interpolate is inverted!"); 
    #endif

    }

But sometimes the drift becomes unpredictable, and as I understand it, I need to use the Lagrange polynomial, but I can’t figure it out enough yet.

[flybrianfly]

BFS_PI and BFS_2PI are constants for PI and 2PI. The WrapToPi and WrapTo2Pi are functions to wrap an angle either to +/-PI or 0 to 2PI. This is useful for heading where some conventions want a +/-180 output and some want a 0 - 360 output.

Orientation of the GPS antenna shouldn't matter. It's measuring the NED velocity directly, not from anything like an embedded magnetometer.

[flybrianfly]

The temperature drift for the accel and gyro is estimated by the EKF, you shouldn't have to worry about it if everything is working well.

[brightproject]

The temperature drift for the accel and gyro is estimated by the EKF, you shouldn't have to worry about it if everything is working well.

With drift, I will try compensation by my, additional algorithms or I’ll use thermostating.
I thought you had encountered problems with the temperature drift of the gyroscope and there were some solutions in the code.
@flybrianfly could you tell me please, based on the values ​​from the sensors, in what units the data from the magnetometer is?
According to the data below

Ax_Mps2: -0.06
Ay_Mps2: -0.18
Az_Mps2: 9.83
Gx_Rad: 0.02
Gy_Rad: 0.06
Gz_Rad: 0.04
Mx_uT: -132.00
My_uT: -200.00
Mz_uT: -1070.00

and according to this data?

 Ax_Mps2: -0.07
 Ay_Mps2: -0.17
 Az_Mps2: 9.84
 Gx_Rad: 0.02
 Gy_Rad: 0.06
 Gz_Rad: 0.04
 Mx_uT: -1.56
 My_uT: -1.90
 Mz_uT: -10.82

It seems to me that the most recent values are more like uT.
I received them in the following way:

     mag_ut[0] = SENSOR_SIGN[6] * x;
     mag_ut[0] *= 0.01f; // uT
     mag_ut[1] = SENSOR_SIGN[7] * y;
     mag_ut[1] *= 0.01f; // uT
     mag_ut[2] = SENSOR_SIGN[8] * z;
     mag_ut[2] *= 0.01f; // uT

The EKF algorithm does not react in any way to a change in value from the magnetometer.
As far as I understand, the data must be submitted to micro Tesla?
Can you please clarify that I scale data from the accelerometer in G, and from the gyroscope in rad/s.
But when the board is moved in roll or pitch by 90 degrees, the resulting angle from the algorithm is about 110 degrees.
Something with the integration coefficient apparently.
I installed it from the example
static constexpr float DT = 1.0f / 100.0f;
I also tried this format
static constexpr float DT = 1e-2f;
My values are the following:

#define GYRO_RANGE 2000.0f
#define ACCEL_RANGE 8.0f

But I saw in the code that the accelerometers are set to 16G, is this important?
Also, during these manipulations, I move the board with accelerometer, gyroscope and magnetometer sensors, but do not move the GNSS module with the antenna.
For example, when I start to rotate the board at an angle from 0 to 90 degrees, the angle at the exit from the filter I get, as I wrote above, is 110 degrees, but when I leave the board at a position of 90 degrees in roll, the angle from 110 begins to decrease to 90.
Perhaps you need to move the board and at the same time move the GNSS receiver?
And as far as I understand from previous discussions on the pjrc forum, it is difficult to check the EKF filter in a static position.
Does it need dynamics?
Can I ask one more question?
You are using the IMU_SRD parameter.
As I understand it, this is a gyro rate parameter
https://github.com/bolderflight/invensense-imu/blob/2255d746246e8f5d4a6269e68db1fcaaf34aaade/src/mpu9250.cpp#L239
How many times per second will data be read from the sensor?
I am using an LSM6DS3 gyro and accelerometer sensor and my library does not have the ability to set the frequency of reading from the sensor.
I am using the code in an infinite loop, can you tell me if it can be equivalent to using IMU_SRD in your code?

void loop() {
  /* Check for new GNSS data */
  if (gnss.Read()) {
    new_gnss_data = true;
    llh[0] = gnss.lat_rad();
    llh[1] = gnss.lon_rad();
    llh[2] = gnss.alt_wgs84_m();
    ned_vel_mps[0] = gnss.north_vel_mps();
    ned_vel_mps[1] = gnss.east_vel_mps();
    ned_vel_mps[2] = gnss.down_vel_mps();
  }
  /* Check for new IMU data and run EKF */
// if (imu.Read()) {
if (millis() - previousMillis >= 10)
{
    previousMillis = millis();
// not plausible to read, simplified to reduce code.
    accel_mps2[0];
    accel_mps2[1];
    accel_mps2[2];

    gyro_radps[0];
    gyro_radps[1];
    gyro_radps[2];

    new_mag_data = mag_read();

    if (new_mag_data) {

    mag_ut[0];
    mag_ut[1];
    mag_ut[2];

    }
    /* Initialize and run the EKF */
    if (!ekf_initialized) {
      if (new_mag_data && new_gnss_data && (gnss.fix() >= 3) &&
      (gnss.num_sv() > 5)) {
        ekf.Initialize(accel_mps2, gyro_radps, mag_ut, ned_vel_mps, llh);
        ekf_initialized = true;
    digitalWrite(STATUS_LED, !digitalRead(STATUS_LED)); 
      }
    } else {
      /* Time update occurs every frame since we're tied to IMU read */
      ekf.TimeUpdate(accel_mps2, gyro_radps, DT);
      /* Measurement update if new GNSS data */
      if (new_gnss_data) {
        ekf.MeasurementUpdate(ned_vel_mps, llh);
      }
    }
    /* Reset the new GNSS data flag */
    if (new_gnss_data) {
      new_gnss_data = false;
    }

    /* Output Euler angles */
    Serial.print("Orientation: ");
    Serial.print(-bfs::rad2deg(ekf.roll_rad())); // pitch
    Serial.print(" ");
    Serial.print(-bfs::rad2deg(ekf.pitch_rad())); // roll
    Serial.print(" ");
    Serial.println(bfs::rad2deg(ekf.yaw_rad()));


  }
}

In your code example, the condition
if (imu.Read())
executed every 10 ms, i.e. with a frequency of 100Hz.
I have a condition
if (millis() - previousMillis >= displayPeriod)
also runs every 10ms.
But during operation, the orientation angles “slow down” a little and spasmodically.
I managed to purchase a neo-8m GNSS receiver (neo-6m does not work with the ublox library because it cannot issue NAV-PVT messages).
When I ran the test ublox_example
The data received in the serial port also slowed down a little in places.
The GNSS receiver antenna is located in the apartment, but it picks up 11 satellites.
I adjusted the sampling frequency from neo-8m to 5 Hz.
And I can’t understand whether this lag is caused by calculations from the GPS or something is wrong in the time calculations.
Regarding the orientation of the axes.
The algorithm accepts the NED convention:

My board with sensors have next orientation of the gyroscope, accelerometer and magnetometer axes is as follows:

Did I set the sign of each axis and the direction of rotation correctly?

+Ax, -Ay, -Az
+Gx, -Gy, -Gz
+Mx, +My, -Mz

I'm also wondering if the range of pitch angles should be from 180 to 180, and the roll angles from 90 to 90 or vice versa?

[Juaxs2022]

Hello Brian,

Quick question, do the Arduino example work with the ESP32 board?

[flybrianfly]

The 15 state EKF as implemented in this library does not work with the ESP32. I do have a version that does, but haven't had a chance to test to see what effect those changes have.

[flybrianfly]

For the ESP32, I had to split line 259 of the 15 state EKF code into several temporary variables to get it to work:

    temp1_ = (Eigen::Matrix<float, 15, 15>::Identity() - k_ * h_) * p_;
    temp2_ = (Eigen::Matrix<float, 15, 15>::Identity() - k_ * h_).transpose();
    temp3_ = temp1_ * temp2_;
    temp4_ = k_ * r_ * k_.transpose();
    p_ = temp3_ + temp4_;