Arduino EKF implementation #4
Comments
|
Unfortunately, we do not have an Arduino version of this library implemented yet. |
|
@flybrianfly The only namespace I see used is bfs in the source or am I missing something |
|
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: Going to assume a few things with this next commet. In setup I will need to do a: |
|
@mjs513 are you using the latest version downloaded from GitHub? I don't see a types.h include and there shouldn't be one. |
|
@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();
}
}
|
|
Hi Brian 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 :) |
Oops missed this comment - its in the arduino and cmake examples - its currently commented out |
|
@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. |
|
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. |
|
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. |
|
@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. |
|
@flybrianfly |
|
@mjs513, I typically use either a SAM-M8Q or a ZED-F9P |
|
@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 |
|
@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. |
Good day @mjs513 and @flybrianfly
I had to do it in two lines Line 227 in 9029646 and here Line 405 in 9029646 Replace G_MPS2<float> by G_MPS22. Also a compilation error There is a separate quest, in the units library, it says to use such constants I am using LSM6DS3(gyroscope and accelerometer) and QMC5883(magnetometer) sensors.
Examplecode compiled.
My board with sensors is located like this, and the axes are as indicated in the code. I would like examples of setting parameters Line 331 in 9029646 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. But sometimes the drift becomes unpredictable, and as I understand it, I need to use the |
|
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. |
|
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. and according to this data? It seems to me that the most recent values are more like The But I saw in the code that the accelerometers are set to In your code example, the condition 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? 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? |
|
Hello Brian, Quick question, do the Arduino example work with the ESP32 board? |
|
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. |
|
For the ESP32, I had to split line 259 of the 15 state EKF code into several temporary variables to get it to work: |
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.
The text was updated successfully, but these errors were encountered: