Bandwidth, rates and calibration

[kulve]

Hi Kris,

Thank you for your information about the sensor fusion topic. Filing an issue against your code is quite misleading as I'm not even using it (directly anyway) but I'm hoping you have some suggestions for me.

I've been trying to get my own 168MHz STM32F4/ChibiOS based setup running for quite some time without success. It reacts well to change (I guess my gyroscope math are working properly) but when I stop rotating the sensor, yaw typically crawls for the next 10 seconds to some value (something wrong with my magnetometer math?).

Just in case you are interested, here's my lsm9ds0 driver:
https://github.com/snowcap-electronics/control-board/blob/master/drivers/sc_lsm9ds0.c

And my copy'paste of the original Madwick's algorithm (with some googled updates, I'll probably update that to match yours):
https://github.com/snowcap-electronics/control-board/blob/master/src/sc_ahrs.c

To the questions:

1. I have set all sensors to roughly 100Hz (gyro is 95Hz). I'm waiting to get a reading from all of them and then I run the AHRS math. I noticed that you are using quite different rates for each of them and run the AHRS math when ever you get a reading from any of them. Why 10Hz (or 25Hz) magnetometer rate instead of 100Hz? Do you happen to have any pointers to docs about rate and bandwidth and how to pick them?

2. I just realised that it seems that in magnetometer calibration you want to find the maximums for each axis in each direction and then calculate the "origin" as the bias:

https://github.com/kriswiner/LSM9DS0/blob/master/Teensy3.1/LSM9DS0-MS5637/LSM9DS0_MS5637_Mini_Add_On.ino#L808

Or did I understand that wrong? I'm not doing that at all. How big impact could that give? Could that alone be the reason why my yaw crawls so much when I stop rotating it?

3. I noticed that LSM9DS0 and LSM9DS1 have their magnetometer Z-axis pointing in opposite directions. Does the AHRS algorithm assume some direction? Currently I'm assuming all sensors point to right directions for the Madwick's algorithm and I haven't been able to confirm if that's ok.

So are the correct sensor axis directions listed somewhere?

From e.g.:
https://github.com/sparkfun/LSM9DS0_Breakout/blob/master/Libraries/Arduino/SFE_LSM9DS0/examples/LSM9DS0_AHRS/LSM9DS0_AHRS.ino#L267

// Sensors x- and y-axes are aligned but magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro!
// This is ok by aircraft orientation standards!

Thank you!

[kriswiner]

Hi Tuomas,

I have been a little sloppy in the way I hand data to the sensor fusion
algorithm. The right way to do so is to use the FIFO to gather data at a
prescribed rate and then use the FIFO timestamp to send the sensor fusion
algorithm time-aligned data for processing. The problem with this strategy
is that the FIFO rarely accepts magnetometer data. But thatis usually OK
anyway since the magnetometer is somewhat noisy and power-hungry, so running
at 10 Hz or 25 Hz, as I do, is good enough. The job of the mag data is to
correct the gyro drift and it can do this job at a slower-than-200 Hz data
rate, since the drift is rather slow.

But, yes, you must recenter the magnetometer response surface to the origin
for any kind of heading (yaw) accuracy and drift minimization. Ideally you
would also re-spherize the response surface but this requires a lot of
measurements and a matrix of corrective biases so most people don't. Except
for the most precise applications this is likely unnecessary too.

You must get the axes alignment right when handing to the sensor fusion
algorithm. The scaling is arbitrary sine the Madgwick and Mahony filters
normalize the data, but if you tell the algorithm that positive changes in
the x magnetometer reading, for example, are to be interpreted as negative
changes in the y-axis magnetic field, you will get nonsense results. The
correct strategy here is to pick and axis (usually the x-axis) to be your
reference, then send the sensor fusion algorithm properly aligned data. Say
the x/y/z axis of the accelerometer is aligned NED (North = x, East = y,
Down = z, one standard), then if the minus y-axis of the magnetometer is
aligned in the same direction as the accelerometer x-axis (North when
ideally aligned) then you must pass the sensor fusion algorithm x-axis
magnetic field data that is the raw (or scaled if you do) minus y axis
magnetic sensor data. Confusing, yes. But the idea is if the sensor is
aligned with the accelerometer x-axis in the North direction and you fix the
yaw and change the roll or pitch, the yaw should be unaffected. The only way
for this to happen is if the axis of the magnetometer actually aligned with
the North reference, whatever that happens to be on your sensor of choice,
shows no change. So, bottom line, getting the sensor axes right in the feed
to the sensor fusion algorithm is crucial.

My general rule of thumb is to pick the sample rate to be four or five times
less than the sensor fusion rate. The sensor fusion algorithm is iterative
and requires four or five iterations to achieve a stable solution. So if
your 168 MHz processor can achieve a 5000 Hz sensor fusion rate, run the
gyro and accelerometer at 1000 Hz, if you can. Then the bandwidth should be
10 - 20% of the sample rate. So at 1000 Hz sample rate, try 100 or 200 Hz
bandwidth. These "rules" are flexible and you might modify them to your
particular application. But running at 200 Hz data rates and 400 Hz
bandwidth is just nonsense, for example.

Hope this helps, and please ask other questions as you have them.

Kris
Pesky Products
-----Original Message-----
From: Tuomas Kulve [mailto:notifications@github.com]
Sent: March 23, 2015 11:14 PM
To: kriswiner/LSM9DS0
Subject: [LSM9DS0] Bandwidth, rates and calibration (#8)

Hi Kris,

Thank you for your information about the sensor fusion topic. Filing an
issue against your code is quite misleading as I'm not even using it
(directly anyway) but I'm hoping you have some suggestions for me.

I've been trying to get my own 168MHz STM32F4/ChibiOS based setup running
for quite some time without success. It reacts well to change (I guess my
gyroscope math are working properly) but when I stop rotating the sensor,
yaw typically crawls for the next 10 seconds to some value (something wrong
with my magnetometer math?).

Just in case you are interested, here's my lsm9ds0 driver:
https://github.com/snowcap-electronics/control-board/blob/master/drivers/sc_
lsm9ds0.c

And my copy'paste of the original Madwick's algorithm (with some googled
updates, I'll probably update that to match yours):
https://github.com/snowcap-electronics/control-board/blob/master/src/sc_ahrs
.c

To the questions:

1. I have set all sensors to roughly 100Hz (gyro is 95Hz). I'm waiting to
   get a reading from all of them and then I run the AHRS math. I noticed that
   you are using quite different rates for each of them and run the AHRS math
   when ever you get a reading from any of them. Why 10Hz (or 25Hz)
   magnetometer rate instead of 100Hz? Do you happen to have any pointers to
   docs about rate and bandwidth and how to pick them?

2. I just realised that it seems that in magnetometer calibration you want
   to find the maximums for each axis in each direction and then calculate the
   "origin" as the bias:

https://github.com/kriswiner/LSM9DS0/blob/master/Teensy3.1/LSM9DS0-MS5637/LS
M9DS0_MS5637_Mini_Add_On.ino#L808

Or did I understand that wrong? I'm not doing that at all. How big impact
could that give? Could that alone be the reason why my yaw crawls so much
when I stop rotating it?

3. I noticed that LSM9DS0 and LSM9DS1 have their magnetometer Z-axis
   pointing in opposite directions. Does the AHRS algorithm assume some
   direction? Currently I'm assuming all sensors point to right directions for
   the Madwick's algorithm and I haven't been able to confirm if that's ok.

So are the correct sensor axis directions listed somewhere?

From e.g.:
https://github.com/sparkfun/LSM9DS0_Breakout/blob/master/Libraries/Arduino/S
FE_LSM9DS0/examples/LSM9DS0_AHRS/LSM9DS0_AHRS.ino#L267

// Sensors x- and y-axes are aligned but magnetometer z-axis (+ down) is
opposite to z-axis (+ up) of accelerometer and gyro!
// This is ok by aircraft orientation standards!

Thank you!

Reply to this email directly or view it on GitHub
#8 .
<https://github.com/notifications/beacon/AGY1qjyHkTIMvKetDmlSDmEvCfdvInJZks5
n4Pg7gaJpZM4DzlhB.gif>

[kulve]

Thanks for the explanations.

I think my axis are correct as I'm using the same sensor as you are and I don't see you doing any swapping of them.

I started disconnecting the AHRS loop from the sensor loop. Next would be changing the rate and bandwidth settings to match yours. And finally I'll add the magnetometer calibration.

That'll take awhile but I'll let you know how it goes.

[djsg]

Hi kris,

Based on your 9250 code, I wrote a piece of firmware for my STMF401 sensor board which is connected with a MPU9250 through SPI. The obtained Quaternion values are as showed below:

ax = 4.211426 ay = -39.550781 az = -23.498535 mg
gx = 1.854966 gy = -0.740454 gz = 0.687027 deg/s
gx = -704.388916 gy = -356.964600 gz = -345.222534 mG
temperature = 20.997005 C
q0 = 0.416777
q1 = -0.695210
q2 = -0.463045
q3 = 0.358565
Yaw, Pitch, Roll: 57.775761 6.464238 -113.452301
average rate = 493125.218750
-- 500ms --
ax = 4.211426 ay = -39.550781 az = -23.498535 mg
gx = 1.854966 gy = -0.740454 gz = 0.687027 deg/s
gx = -704.388916 gy = -356.964600 gz = -345.222534 mG
temperature = 20.997005 C
q0 = 0.416761
q1 = -0.695179
q2 = -0.463089
q3 = 0.358587
Yaw, Pitch, Roll: 57.782719 6.463478 -113.451973
average rate = 493125.218750
... ...
Comparing your result, which almost remain the same for half an hour, my results changes rather fast. Can you tell me what could be the possible causes?

FYI, below I attached the printf messages during the init stage:
x gyro bias = -2.480916
y gyro bias = 0.519084
z gyro bias = -0.267176
x accel bias = -0.005310
y accel bias = 0.032104
z accel bias = 0.026733

MPU9250 initialized for active data mode....
6500 user control register = 64
force spi mode
reset mag
mag_asa 182, 184, 171
compass who am i 72. is it 72?

magCalibration {72.0000, 184.0000, 171.0000}
AK8963 initialized for active data mode....
Accelerometer full-scale range = 2.000000 g
Gyroscope full-scale range = 250.000000 deg/s
Magnetometer resolution = 16 bits
Magnetometer ODR = 100 Hz
Accelerometer sensitivity is 16384.000000 LSB/g
Gyroscope sensitivity is 131.072006 LSB/deg/s
Magnetometer sensitivity is 0.776487 LSB/G

Sorry, I am new in this IMU Quaternion concept; have little idea on how to verify the data I get.

[kriswiner]

Well, there seems to be a problem with the accelerometer. If the sensor is
lying still and flat on a table it should read ~1000 mg in the z-direction
and ~0 in the x and y directions. The mag data also seems a little weird, I
don't expect all three axes to be negative. But your accel data is really
off. Your sensor fusion rate is too high, it's as if you are not reading the
data at all and are just repeating jibberish.

Once you get the accel straightened out,you can work on the mag
calibrations. Are you using a figure eight motion to capture max/min to
recenter the mag response sphere, or something else? Proper mag calibration
is critical to getting reasonable sensor fusion accuracy.

Kris
-----Original Message-----
From: djsg [mailto:notifications@github.com]
Sent: April 15, 2015 1:07 AM
To: kriswiner/LSM9DS0
Cc: Kris Winer
Subject: Re: [LSM9DS0] Bandwidth, rates and calibration (#8)

Hi kris,

Based on your 9250 code, I wrote a piece of firmware for my STMF401 sensor
board which is connected with a MPU9250 through SPI. The obtained Quaternion
values are as showed below:

ax = 4.211426 ay = -39.550781 az = -23.498535 mg
gx = 1.854966 gy = -0.740454 gz = 0.687027 deg/s
gx = -704.388916 gy = -356.964600 gz = -345.222534 mG
temperature = 20.997005 C
q0 = 0.416777
q1 = -0.695210
q2 = -0.463045
q3 = 0.358565
Yaw, Pitch, Roll: 57.775761 6.464238 -113.452301
average rate = 493125.218750
-- 500ms --
ax = 4.211426 ay = -39.550781 az = -23.498535 mg
gx = 1.854966 gy = -0.740454 gz = 0.687027 deg/s
gx = -704.388916 gy = -356.964600 gz = -345.222534 mG
temperature = 20.997005 C
q0 = 0.416761
q1 = -0.695179
q2 = -0.463089
q3 = 0.358587
Yaw, Pitch, Roll: 57.782719 6.463478 -113.451973
average rate = 493125.218750
... ...
Comparing your result, which almost remain the same for half an hour, my
results changes rather fast. Can you tell me what could be the possible
causes?

FYI, below I attached the printf messages during the init stage:
x gyro bias = -2.480916
y gyro bias = 0.519084
z gyro bias = -0.267176
x accel bias = -0.005310
y accel bias = 0.032104
z accel bias = 0.026733

MPU9250 initialized for active data mode....
6500 user control register = 64
force spi mode
reset mag
mag_asa 182, 184, 171
compass who am i 72. is it 72?

magCalibration {72.0000, 184.0000, 171.0000}
AK8963 initialized for active data mode....
Accelerometer full-scale range = 2.000000 g
Gyroscope full-scale range = 250.000000 deg/s
Magnetometer resolution = 16 bits
Magnetometer ODR = 100 Hz
Accelerometer sensitivity is 16384.000000 LSB/g
Gyroscope sensitivity is 131.072006 LSB/deg/s
Magnetometer sensitivity is 0.776487 LSB/G

Sorry, I am new in this IMU Quaternion concept; have little idea on how to
verify the data I get.

Reply to this email directly or view it on GitHub
#8 (comment) .
<https://github.com/notifications/beacon/AGY1qupN4pO3dAkYLjejmqSpbwSHpT9kks5
n_hOogaJpZM4DzlhB.gif>

[djsg]

yeah, you are right. The Accelerometer readings are far way off.
I will look into it.

[kulve]

I've finally got the "disconnected" AHRS loop working and I've been experimenting with magnetometer calibration as well. No real improvements though.

I've understood that I should be able to calculate the magnetic heading with atan(y/x) assuming the sensor is flat and still on the table? How accurate should that be?

Comparing my calibrated magnetometer heading to a traditional compass, I get -20° difference when pointing to magnetic north, -7° pointing east, +21° pointing south and +8° when pointing west. I assume it shouldn't be that far off?

I've ordered one IMU shield now from Sparkfun just to compare it to this my self-made.

[kriswiner]

I would recommend that you plot Mx vs Mz and My vs Mz for your magnetometer to see how bad the offset and skew are. I suspect your mag is not properly calibrated and this will lead to large deviations of the heading.

I have been doing some testing with the LSM9DS0 whose results you can see here:

https://github.com/kriswiner/MPU-6050/wiki/9-DoF-Motion-Sensor-Bakeoff

The tangent is an approximation, you should rather derive the Euler angles from the quaternion that results from sensor fusion.

I am disappointed that you didn't buy one of my LSM9DS0 breakout at my Tindie store ;/!

Did you use my LSM9DS0 sketch? There are mag calibration and open-source sensor fusion algorithms there that might be useful.

You should be able to get better heading results that this.

[kulve]

I've plotted the values and tried to calibrate the magnetometer by applied the offset and scaling based on the minimums and maximums.

http://tuomas.kulve.fi/tmp/magn-values-sphere-lsm9ds0-realraw.png

I think your LSM9DS0 breakout board might be better as it doesn't have the GND plane below the sensor while the Sparkfun's sensor does. I bought the Photon LSM9DS1 shield as I'm planning to use other Photon shields as well.

I'm using your open-source sensor fusion algorithms but the calibration I did manually with spreadsheets and hard coded the values. And I calculated the simple heading just to test the magnetometer behaviour as I still see some "crawling" along the yaw axis. Pitch and roll behave really well but yaw slowly crawls some 10-40° after I stop rotating the sensor.

I had to reverse the magnetometer z-axis of my LSM9DS0 to get roughly correct yaw direction when I change the pitch of the sensor. Without reversing, the yaw always changed a lot when I tilted the sensor. I'm still puzzled why I had to do that because I don't see you doing anything similar in your code with LSM9DS0 (I did notice you do that for LSM9DS1 as it's z-axis is reversed compared to LSM9DS0).

[kriswiner]

It looks like you still have significant offset in your mag data as well as the expected skew. You might need to collect min/max data for a longer time or do the calibration dynamically to account for changes in either the magnetometer or its environment.

You really need 9 DoF sensor fusion to avoid the kind of yaw drift you describe. The magnetometer will dynamically correct the gyro drift which leads directly to yaw crawl. See if the yaw is better behaved with the Madgwick or Mahoney fusion filter.

Kris

[kulve]

Just FYI: I changed my plans and ordered your LSM9DS1 shield with the MS5611 pressure sensor.

[chaitroller]

Hi Tuomas, Kris,

Please help me. I am using LSM9DS0 in my project; I am facing the exact same issue Tuomas mentioned in his post on July 13 2015 : There is a error in magnetic heading calculated using atan(y/x); the error is as large as 30 degrees between traditional compass and the heading from LSM9DS0. I have tried AHRS.

For all the testing I am keeping LSM9DS0 flat on the surface; so there is no tilt. Can you please share, how did you resolve above issue?

[kulve]

Sorry for the slow reply. I didn't solve my issues even with the LSM9DS1 sensor. I do plan to try again later when I again have more time.

[kriswiner]

I don't really remember what the specific issue was but most issues can be
resolved with good sensor calibration and a faster MCU.

-----Original Message-----
From: Tuomas Kulve [mailto:notifications@github.com]
Sent: September 14, 2016 4:42 AM
To: kriswiner/LSM9DS0
Cc: Kris Winer; Comment
Subject: Re: [kriswiner/LSM9DS0] Bandwidth, rates and calibration (#8)

Sorry for the slow reply. I didn't solve my issues even with the LSM9DS1
sensor. I do plan to try again later when I again have more time.

You are receiving this because you commented.
Reply to this email directly, view it on GitHub
#8 (comment) , or
mute the thread
<https://github.com/notifications/unsubscribe-auth/AGY1qnzC4Wo3cvRFm9vdCTed6
SVNhh8Jks5qp92ggaJpZM4DzlhB> .
<https://github.com/notifications/beacon/AGY1qsDAySFDq7cJIwzelr1JV6bUz8NJks5
qp92ggaJpZM4DzlhB.gif>