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.