I have searched all over the find python code for converting raw magnometer readings from the InvenSense ICM20948 9DoF 9 axis sensor. I have tried adapting some base code that converts to Gauss and some other code written for the LSM 303. I am having no luck. Using the LSM303 code from Tero Karvinen's Make: Sensors I get something close but there is obviously something wrong in the vector math as the headings are not right even after calibration.
Summary is I need Python code for Raspberry Pi to convert raw magnomenter and accelerometer readings into headings. Any suggestions on where I can find this?
thank you
InvenSense ICM-20948 9-DoF IMU Compass Heading
Moderators: adafruit_support_bill, adafruit
Please be positive and constructive with your questions and comments.
- bradallen
- Posts: 8
- Joined: Wed Sep 12, 2018 10:56 pm
- adam_g
- Posts: 66
- Joined: Sat Feb 18, 2017 11:41 pm
Re: InvenSense ICM-20948 9-DoF IMU Compass Heading
Hi Brad,
Did you ever have any luck finding code to convert the raw magnetometer readings into a heading measurement?
This appears to be lacking (in addition to pitch and roll) for both Arduino and Python.
Cheers,
Adam
Did you ever have any luck finding code to convert the raw magnetometer readings into a heading measurement?
This appears to be lacking (in addition to pitch and roll) for both Arduino and Python.
Cheers,
Adam
- bradallen
- Posts: 8
- Joined: Wed Sep 12, 2018 10:56 pm
Re: InvenSense ICM-20948 9-DoF IMU Compass Heading
No, I have not successfully found anything. I have attempted a ground up solution using code for the LM303 as a starting point. This has been mostly unsuccessful. Currently I can get a bearing but it is far from consistent when I roll or pitch the device. I am pretty frustrated and any help would be greatly appreciated.
- adam_g
- Posts: 66
- Joined: Sat Feb 18, 2017 11:41 pm
Re: InvenSense ICM-20948 9-DoF IMU Compass Heading
Hi Brad,
Sorry to hear you haven't made any progress.
I've also used the LSM303 and Pololu's library for pitch, roll and tilt-compensated heading measurements for a number of years now. While the pitch and roll are relatively straight forward, I have to admit that Polulu's method of calculating the heading is beyond my understanding.
Tilt-compensated heading is really what I'm after most, but it seems like the ICM-20948 may not be the best-suited IMU for this. I've searched for months and haven't found a reliable example of working code. drcpattison comes the closest with his sensor fusion code, but it's not quite there yet. I may have to consider switching to another more user-friendly IMU, such as a Bosch BNO080, or something similar.
Cheers,
Adam
Sorry to hear you haven't made any progress.
I've also used the LSM303 and Pololu's library for pitch, roll and tilt-compensated heading measurements for a number of years now. While the pitch and roll are relatively straight forward, I have to admit that Polulu's method of calculating the heading is beyond my understanding.
Tilt-compensated heading is really what I'm after most, but it seems like the ICM-20948 may not be the best-suited IMU for this. I've searched for months and haven't found a reliable example of working code. drcpattison comes the closest with his sensor fusion code, but it's not quite there yet. I may have to consider switching to another more user-friendly IMU, such as a Bosch BNO080, or something similar.
Cheers,
Adam
- gammaburst
- Posts: 1015
- Joined: Thu Dec 31, 2015 12:06 pm
Re: InvenSense ICM-20948 9-DoF IMU Compass Heading
If you combine only accel and magneto, the heading value will be very sensitive to vibration, and you may be unhappy.
For better results, combine the accel/gyro/magneto sensors into an orientation vector (sensor fusion). First, calibrate the sensors (it may be sufficient to simply subtract away the sensor's obvious offset errors). Then apply fusion. Try Adafruit's AHRS library and examples (the library includes Mahony, Madgwick, and maybe other algorithms): https://learn.adafruit.com/how-to-fuse- ... uaternions. A couple years ago I did that with an Arduino and various 9-axis sensors, and they all worked similarly. I haven't tried python though, don't know much about it.
Or are you having difficulty pulling together all those steps?
For better results, combine the accel/gyro/magneto sensors into an orientation vector (sensor fusion). First, calibrate the sensors (it may be sufficient to simply subtract away the sensor's obvious offset errors). Then apply fusion. Try Adafruit's AHRS library and examples (the library includes Mahony, Madgwick, and maybe other algorithms): https://learn.adafruit.com/how-to-fuse- ... uaternions. A couple years ago I did that with an Arduino and various 9-axis sensors, and they all worked similarly. I haven't tried python though, don't know much about it.
Or are you having difficulty pulling together all those steps?
- bradallen
- Posts: 8
- Joined: Wed Sep 12, 2018 10:56 pm
Re: InvenSense ICM-20948 9-DoF IMU Compass Heading
Thank you for your help on this. I too may be switching to something else. I am migrating a system from Arduino to RPi. I had used the BN008 on several projects and it works fairly well. There is a UART conflict for using that with RPi so I went after a different solution. I think I might work at some sort of Arduino/RPi interface. I was actually very disappointed that Adafruit's library did not include this functionality.
Thank you again for the help.
Thank you again for the help.
- adam_g
- Posts: 66
- Joined: Sat Feb 18, 2017 11:41 pm
Re: InvenSense ICM-20948 9-DoF IMU Compass Heading
Hi Brad,
It seems both Adafruit and SparkFun have been struggling with implementing sensor fusion on the ICM-20948. I've heard SparkFun comment that InvenSense actually has their DMP code behind an NDA, so they weren't actually able to implement this functionality in their library. If this is true, it's quite BANNED, especially since the ICM-20948 is touted as the next and greatest. I've tried contacting InvenSense directly but never received a response.
I'll be sure to keep you posted if I make any progress. The BN0080 (https://www.sparkfun.com/products/14686) and BNO085 (https://www.adafruit.com/product/4754) can both operate over I2C. Would that be a possible solution to solve your UART conflict?
Cheers,
Adam
It seems both Adafruit and SparkFun have been struggling with implementing sensor fusion on the ICM-20948. I've heard SparkFun comment that InvenSense actually has their DMP code behind an NDA, so they weren't actually able to implement this functionality in their library. If this is true, it's quite BANNED, especially since the ICM-20948 is touted as the next and greatest. I've tried contacting InvenSense directly but never received a response.
I'll be sure to keep you posted if I make any progress. The BN0080 (https://www.sparkfun.com/products/14686) and BNO085 (https://www.adafruit.com/product/4754) can both operate over I2C. Would that be a possible solution to solve your UART conflict?
Cheers,
Adam
- gammaburst
- Posts: 1015
- Joined: Thu Dec 31, 2015 12:06 pm
Re: InvenSense ICM-20948 9-DoF IMU Compass Heading
I whipped up this crude Arduino Uno sketch that reads an ICM-20948 and outputs Euler angles (heading roll pitch) using only accelerometer and magnetometer (no gyro). I think this answers bradallen's original question, except it's not python. However, you probably won't like the noise and sensitivity to acceleration, so I recommend including the gyro and trying Adafruit's AHRS library.
You don't have to download the fusion/AHRS algorithm into the sensor, although that would be nice.
Be sure to read my code comment about calibration.
You don't have to download the fusion/AHRS algorithm into the sensor, although that would be nice.
Be sure to read my code comment about calibration.
Code: Select all
#include <ICM_20948.h>
#define AD0_VAL 1 // last bit of I2C address
ICM_20948_I2C myicm;
void setup()
{
Serial.begin(115200);
while (!Serial) {};
Wire.begin();
Wire.setClock(400000);
myicm.begin(Wire, AD0_VAL);
}
void loop()
{
if (myicm.dataReady())
{
myicm.getAGMT();
float ax = -myicm.accX() * 0.001 + 0.000; // these calibration offsets are for MY sensor, you must substitute offsets for YOUR sensor
float ay = myicm.accY() * 0.001 + 0.010;
float az = -myicm.accZ() * 0.001 + 0.040;
float gx = -myicm.gyrX() - 0.400;
float gy = myicm.gyrY() - 0.730;
float gz = -myicm.gyrZ() + 0.300;
float mx = -myicm.magX() - 5.00;
float my = -myicm.magY() - 2.50;
float mz = myicm.magZ() - 9.50;
float roll = atan2(-ay, -az); // positive is left wing up
float pitch = atan2(+ax, sqrt(ay*ay + az*az)); // positive is nose up
float heading = atan2(-my*cos(roll) + mz*sin(roll), mx*cos(pitch) + my*sin(pitch)*sin(roll) + mz*sin(pitch)*cos(roll)); // positive is nose right
Serial.print("Orientation: ");
Serial.print(180/PI*heading); Serial.print(" ");
Serial.print(180/PI*roll); Serial.print(" ");
Serial.print(180/PI*pitch); Serial.println();
}
}
- gammaburst
- Posts: 1015
- Joined: Thu Dec 31, 2015 12:06 pm
Re: InvenSense ICM-20948 9-DoF IMU Compass Heading
Here's an example sketch that reads an ICM-20948 (sparkfun library) and does Mahony fusion (Adafruit library) with Euler output. It runs on an Arduino Uno at 100 Hz. I threw this together quickly, so beware!
This sketch uses the gyro, so it almost eliminates the noise and acceleration sensitivity of the accel+magneto solution (my previous example sketch). However, Adafruit's AHRS library doesn't properly initialize the Mahony algorithm at startup, so the orientation takes many seconds (sometimes minutes) to settle down if the sensor wasn't pointed north at startup. Maybe I'll fix it someday, not today.
I'm testing these sketches by sending the Euler output to a 3D graphics display that renders a model airplane in real time.
Read my code comment about calibration offsets. Important!
Maybe someone will find these quickie example sketches useful.
This sketch uses the gyro, so it almost eliminates the noise and acceleration sensitivity of the accel+magneto solution (my previous example sketch). However, Adafruit's AHRS library doesn't properly initialize the Mahony algorithm at startup, so the orientation takes many seconds (sometimes minutes) to settle down if the sensor wasn't pointed north at startup. Maybe I'll fix it someday, not today.
I'm testing these sketches by sending the Euler output to a 3D graphics display that renders a model airplane in real time.
Read my code comment about calibration offsets. Important!
Maybe someone will find these quickie example sketches useful.
Code: Select all
// I threw this together quickly, so beware!
// Reads ICM_20948 using Sparkfun library. Does fusion using Adafruit AHRS library. Outputs Euler angles.
// Runs at 100 Hz on Arduino Uno.
// I flipped some axes because I prefer NED orientation.
#include <ICM_20948.h> // I'm using sparkfun's ICM20948 library (I couldn't get Adafruit's library to work well)
#include <Adafruit_AHRS.h> // library works, except orientation takes many seconds to settle down if sensor wasn't pointed north at startup
#define AD0_VAL 1 // I2C address LSB, you may need to change this for your breakout
ICM_20948_I2C myICM; // ICM_20948_I2C object
ICM_20948_fss_t myFSS; // full scale settings structure that can contain values for all configurable sensors
ICM_20948_dlpcfg_t myDLP; // configuration structure for the desired sensors
Adafruit_Mahony filter;
#define SAMPLERATE_HZ 100 // 100 Hz works fine on Arduino Uno
unsigned long tprev; // time of previous measurement
void setup()
{
Serial.begin(115200);
while (!Serial) {};
Wire.begin();
myICM.begin(Wire, AD0_VAL);
myFSS.a = gpm8; // gpm2(default) gpm4 gpm8 gpm16
myFSS.g = dps2000; // dps250(default) dps500 dps1000 dps2000
myICM.setFullScale(ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr, myFSS);
#if 1 // enable accel and gyro lowpass filters, is there no mag filter?
myDLP.a = acc_d23bw9_n34bw4; // acc_d473bw_n499bw acc_d246bw_n265bw(default) acc_d111bw4_n136bw acc_d50bw4_n68bw8 acc_d23bw9_n34bw4 acc_d11bw5_n17bw acc_d5bw7_n8bw3 (3dB bandwidth nyquist bandwidth)
myDLP.g = gyr_d23bw9_n35bw9; // gyr_d361bw4_n376bw5 gyr_d196bw6_n229bw8(default) gyr_d151bw8_n187bw6 gyr_d119bw5_n154bw3 gyr_d51bw2_n73bw3 gyr_d23bw9_n35bw9 gyr_d11bw6_n17bw8 gyr_d5bw7_n8bw9 (3dB bandwidth nyquist bandwidth)
myICM.setDLPFcfg(ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr, myDLP);
myICM.enableDLPF(ICM_20948_Internal_Acc, true);
myICM.enableDLPF(ICM_20948_Internal_Gyr, true);
#endif
filter.begin(SAMPLERATE_HZ);
Wire.setClock(400000);
tprev = micros();
}
void loop()
{
myICM.getAGMT();
float ax = -myICM.accX() * 0.001 + 0.000; // these offsets calibrate MY sensor, YOUR sensor needs different offsets
float ay = myICM.accY() * 0.001 + 0.010;
float az = -myICM.accZ() * 0.001 + 0.040;
float gx = -myICM.gyrX() - 0.400;
float gy = myICM.gyrY() - 0.730;
float gz = -myICM.gyrZ() + 0.300;
float mx = -myICM.magX() - 5.00;
float my = -myICM.magY() - 2.50;
float mz = myICM.magZ() - 9.50;
filter.update(gx, gy, gz, -ax, -ay, -az, mx, my, mz); // gyro deg/sec, acc and mag don't care
float roll = filter.getRoll();
float pitch = filter.getPitch();
float heading = filter.getYaw() + 180; // it pointed the wrong way, the 180 fixed it
Serial.print("Orientation: ");
Serial.print(heading); Serial.print(" ");
Serial.print(roll); Serial.print(" ");
Serial.print(pitch); Serial.println();
#define PERIOD_US (unsigned long)round(1000000.0 / SAMPLERATE_HZ)
while (micros() - tprev < PERIOD_US); // wait until next measurement
tprev += PERIOD_US;
}
- adam_g
- Posts: 66
- Joined: Sat Feb 18, 2017 11:41 pm
Re: InvenSense ICM-20948 9-DoF IMU Compass Heading
Hi gammaburst,
Thanks for your examples! I'll give them a shot and see how they work.
drcpattison's example also takes minutes for the heading value to settle down, but also currently has issues with 5-25° being added or subtracted when a 360° rotation is made (see: viewtopic.php?f=19&t=116645&p=842177&hi ... on#p842177).
Cheers,
Adam
Thanks for your examples! I'll give them a shot and see how they work.
drcpattison's example also takes minutes for the heading value to settle down, but also currently has issues with 5-25° being added or subtracted when a 360° rotation is made (see: viewtopic.php?f=19&t=116645&p=842177&hi ... on#p842177).
Cheers,
Adam
- gammaburst
- Posts: 1015
- Joined: Thu Dec 31, 2015 12:06 pm
Re: InvenSense ICM-20948 9-DoF IMU Compass Heading
I haven't seen drcpattison's project. Beware that A LOT of little things can go wrong with these sensor fusion projects, resulting in incorrect/puzzling output. The symptom you described sounds familiar. My first guess is the gyro's scale factor or sample rate don't quite match what the fusion algorithm expects. Or perhaps the sensor offset errors weren't properly removed. Or maybe something else. It helps to see the misbehavior in real-time.
When everything is working properly, you can tilt and twirl the sensor quickly any which way in your hand, send the output data into a 3D graphics display, and it will follow you hand motions with almost no visible delay. It's beautiful to see. My example sketch with Adafruit's AHRS library behaves like that, but only AFTER its startup settling has completed. That's fixable.
When everything is working properly, you can tilt and twirl the sensor quickly any which way in your hand, send the output data into a 3D graphics display, and it will follow you hand motions with almost no visible delay. It's beautiful to see. My example sketch with Adafruit's AHRS library behaves like that, but only AFTER its startup settling has completed. That's fixable.
- gammaburst
- Posts: 1015
- Joined: Thu Dec 31, 2015 12:06 pm
Re: InvenSense ICM-20948 9-DoF IMU Compass Heading
Hi Adam,
Is this the drcpattison example that you are referring to?
https://github.com/drcpattison/DPEng_IC ... on_usb.ino
It's similar to my second example sketch (the one using Adafruit's AHRS library).
I tried it, and it behaved strangely for me until I made these three changes:
1. Changed the calibration offsets to match MY sensor.
2. Changed the "filter.update" call (notice minus signs and swapped mx my):3. Changed the "heading" calculation:
I also recommend reducing or removing the "delay(10)". It's hurting performance.
The sketch now works fine for me (except for that slow startup setting time). However, it's possible that my changes didn't "fix" anything, but merely made it compatible with the NED orientation conventions of my 3D display program. I can't be sure without having your data viewer. There's also a chance that you and I are using different version libraries.
When you conduct your 360 degree rotation test, don't exceed the gyro's rate limit (this sketch selects 250 degrees/sec range). Rotating too fast causes angular error followed by slow drifting to "catch up".
Is this the drcpattison example that you are referring to?
https://github.com/drcpattison/DPEng_IC ... on_usb.ino
It's similar to my second example sketch (the one using Adafruit's AHRS library).
I tried it, and it behaved strangely for me until I made these three changes:
1. Changed the calibration offsets to match MY sensor.
2. Changed the "filter.update" call (notice minus signs and swapped mx my):
Code: Select all
filter.update(gx, -gy, -gz,
-accel_event.acceleration.x, accel_event.acceleration.y, accel_event.acceleration.z,
mx, my, mz);
Code: Select all
float heading = filter.getYaw() + 180;
The sketch now works fine for me (except for that slow startup setting time). However, it's possible that my changes didn't "fix" anything, but merely made it compatible with the NED orientation conventions of my 3D display program. I can't be sure without having your data viewer. There's also a chance that you and I are using different version libraries.
When you conduct your 360 degree rotation test, don't exceed the gyro's rate limit (this sketch selects 250 degrees/sec range). Rotating too fast causes angular error followed by slow drifting to "catch up".
- FedePacio
- Posts: 2
- Joined: Thu May 06, 2021 4:49 am
Re: InvenSense ICM-20948 9-DoF IMU Compass Heading
Good afternoon guys, I'm struggling a little bit with the ICM-20948.
I've read several times that the DPM that is inside should calibrate itself on the background: anyway mine still has some bias even using the provided sketches with Euler Angles.
Hence, looking at gammaburst's posts, I've seen that he's done its calibration himself.
I'm wondering how he's found those offsets: the only guide I've seen is the one whose link is above but seems not working with this IMU.
https://learn.adafruit.com/how-to-fuse- ... -motioncal
Do you have any suggestions on how to get them?
I've read several times that the DPM that is inside should calibrate itself on the background: anyway mine still has some bias even using the provided sketches with Euler Angles.
Hence, looking at gammaburst's posts, I've seen that he's done its calibration himself.
I'm wondering how he's found those offsets: the only guide I've seen is the one whose link is above but seems not working with this IMU.
https://learn.adafruit.com/how-to-fuse- ... -motioncal
Do you have any suggestions on how to get them?
- adam_g
- Posts: 66
- Joined: Sat Feb 18, 2017 11:41 pm
Re: InvenSense ICM-20948 9-DoF IMU Compass Heading
Hi @FedePacio,
While I've given up on the ICM-20948 until InvenSense provides actual useful support to the community, Paul Clark has been making great strides developing SparkFun's library.
https://github.com/sparkfun/SparkFun_IC ... inoLibrary
I haven't yet been able to determine how to calibrate the ICM-20948, though the following GitHub issue may provide some insight:
https://github.com/sparkfun/SparkFun_IC ... /issues/40
Cheers,
Adam
While I've given up on the ICM-20948 until InvenSense provides actual useful support to the community, Paul Clark has been making great strides developing SparkFun's library.
https://github.com/sparkfun/SparkFun_IC ... inoLibrary
I haven't yet been able to determine how to calibrate the ICM-20948, though the following GitHub issue may provide some insight:
https://github.com/sparkfun/SparkFun_IC ... /issues/40
Cheers,
Adam
- FedePacio
- Posts: 2
- Joined: Thu May 06, 2021 4:49 am
Re: InvenSense ICM-20948 9-DoF IMU Compass Heading
Hi @adam_g,
Thank you very much for your fast reply and the links you provided.
Hope InveSense will give more information.. anyway I had already seen Paul Clark’s work and I’m waiting for his reply too but I hope the problem is what you’ve said: InveSense -.-“
Greetings,
Federico.
Thank you very much for your fast reply and the links you provided.
Hope InveSense will give more information.. anyway I had already seen Paul Clark’s work and I’m waiting for his reply too but I hope the problem is what you’ve said: InveSense -.-“
Greetings,
Federico.
Please be positive and constructive with your questions and comments.