Yaw angle different between devices about fusion

[ladislas]

Raw data looks like this

#	dt (ms)	ax	ay	az	gx	gy	gz
0	0	0.006222	-0.016958	0.991982	0.0175	-0.0175	0.0175
1	9	0.001098	-0.016348	0.998082	0.0175	-0.0175	0
2	9	0.006832	-0.02074	0.99979	0.0175	-0.0175	0
3	8	0.00854	-0.016714	1.001254	-5.985	-4.3925	7.42
4	9	0.007198	-0.01891	1.00284	-9.0125	-5.53	6.9125
5	9	0.0061	-0.011712	0.998326	-2.0825	-1.6275	1.5575
6	8	0.005368	-0.019276	1.000522	0.28	-0.28	0.0875
7	9	0.003538	-0.019764	1.00162	0.0875	-0.3325	0.1925
8	8	0.007076	-0.019764	0.998326	0.1925	-0.525	0.315
9	9	0.002074	-0.021594	1.00345	0.0525	-0.4725	0.315
10	9	0.00732	-0.017324	1.002962	0.175	-0.42	0.385

[xioTechnologies]

If you have nothing to add to #58 then please close it. Please share data as plots not tables. You may find it useful to use the x-IMU3 GUI to plot data from your IMU, as described in this comment.

The gyroscope offset correction algorithm does not interact with the accelerometer at all. If you are observing some relationship between the two then this will be due to an error specific to your implementation.

The errors in yaw you have reported for a 180 degree rotation are significantly higher than I would expect for the LSM6DSOX. You will need to do some debugging to understand why this might be happening.

[ladislas]

Thanks a lot! I'll try all that and report here :)

[ladislas]

The gyroscope offset correction algorithm does not interact with the accelerometer at all. If you are observing some relationship between the two then this will be due to an error specific to your implementation.

So I solved this -- we were updating the fusion algorithm using a lambda. moving the code to a function solves the issue, even though I'm not sure why.

If you have nothing to add to #58 then please close it. Please share data as plots not tables. You may find it useful to use the x-IMU3 GUI to plot data from your IMU, as described in #42 (comment).

Thanks for the tip, this tool is great! Here is a screenshot of the calibrated data.

As you can see, no drift and very stable data.

But the rotation error persists. Using the 3D view, it's even easier to see.

I'll ask ST's forum is they know anything about that.

[ladislas]

I've asked the question on ST's forum

https://community.st.com/s/question/0D53W000026dtYVSAY/lsm6dsox-ahrs-yaw-measurement-not-correct

[InputBlackBoxOutput]

Hi @ladislas ,

A variation in yaw measurement is pretty common when only an accelerometer and gyroscope are used. Adding a magnetometer yields better yaw measurement since it uses the earth's magnetic field in a parallel plane to the magnetometer.

This is just an observation. I am not an expert in sensor fusion so don't quote me.

[ladislas]

Thanks @InputBlackBoxOutput for your input (no pun intended). We don't have a magnetometer in the current version of the hardware, it will be added in the next one.

The thing that surprises me is that the variation is constant (from what I can tell in my home made experiment), so it's not like an error is adding up.

I'll keep investigating and report here if I find anything.

[xioTechnologies]

I suggest you do not add a magnetometer. As I said, the error you have reported (a 20° error for a 180° rotation) is significantly higher than I would expect for the LSM6DSOX. I believe there is an issue with your implementation. You should solve this issue rather than compound this error with the additional complexities and errors that a magnetometer may introduce. You should also consider:

• Your product features electric motors and so may simply be incompatible with a magnetometer.
• Magnetometers require additional calibration that may result in unacceptable costs and complexity for a "mass produced" product.

[xioTechnologies]

I suspect the error is the result of how you obtain sensor measurements. It is essential that you read every gyroscope sample and that the value of deltaTime is equal to the time between two samples. From your previous comments, it sounds like your system may be both discarding samples, and providing an inaccurate value of deltaTime.

I am also concerned that the code you shared suggests you may be unfamiliar with thread safety. You are clearly working to a high standard. You and your team conduct code reviews, which is very encouraging. Yet your code was littered with global variables and pointers accessed by multiple threads/ISRs without any asynchronous protection. The inexplicable change in accelerometer value you reported is likely a direct result of this lack of protection. It is critical that you address this, regardless of whether you use Fusion or not.

[ladislas]

I suspect the error is the result of how you obtain sensor measurements. It is essential that you read every gyroscope sample and that the value of deltaTime is equal to the time between two samples. From your previous comments, it sounds like your system may be both discarding samples, and providing an inaccurate value of deltaTime.

Sensor readings are obtain using interrupt "on data ready" -- delta time is consistent with ODR (52Hz)

I am also concerned that the code you shared suggests you may be unfamiliar with thread safety.

Which code are you talking about?

This one, is the one used to obtain the readings above.

https://github.com/leka/LekaOS/blob/develop/spikes/lk_sensors_imu_lsm6dsox_fusion_calibration/main.cpp

That being said, we are still very inexperienced with RTOS and thread safety. We have only been working with Mbed OS 3 years ago.

Yet your code was littered with global variables and pointers accessed by multiple threads/ISRs without any asynchronous protection.

I think the code has changed since you last looked, but I'd greatly appreciate if you could point me to where the issues are.

Regarding "multiple threads", you're right and further review of our driver code showed that some things must further investigated.

The issue with "multiple thread" is that because triggering the interrupt happens in ISR context, we are not able to actually read the sensor in ISR context because of I2C. We need to defer the read in an event queue.

Another thing made me wonder: LSM6DSOX has a large internal FIFO with timestamp -- would it be possible to run Fusion on a bunch of values with timestamp instead of every ODR data ready interrupt? Say every 50ms for example.

[xioTechnologies]

Please can you use the x-IMU3 GUI to log your sensor data for a single 180 degree turn (demonstrating the error). It is important that you include 10 seconds of stationary before and after the turn. Please then share the Inertial.csv file with me here.

I cannot see any asynchronous hazards in your latest code. The only variables shared between contexts in your latest code are fusion::ahrs, fusion::settings, and fusion::global_offset. All three of these variables are only accessed in the main thread before the callback is assigned. The main thread and callback will therefore never attempt to access the variables at the same time.

There is nothing about Fusion that would prevent you from using the FIFO.

[ladislas]

@xioTechnologies here are the data.

Protocol:

• 10 seconds wait
• 180° turn
• 10 seconds wait
• 180° opposite turn
• 10 seconds wait

Data:

• Device 1: very good precision - Device-1-Inertial.csv

• Device 2: ~18° missing for 180° - Device-2-Inertial.csv

[xioTechnologies]

You appear to have made a mistake in the collection of Device-1-Inertial.csv because the first rotation occurs after 1 second, not 10 seconds. The rotation therefore occurs during the algorithm initialisation and so cannot be analysed.

I processed Device-2-Inertial.csv using simple_example.py to obtain the plot below. This plot shows a ~90° rotation. You said that you observe a ~180° rotation in the algorithm output. You must therefore be providing me with data that is different to what you are processing through the algorithm.

I used the following code to plot the difference between each timestamp in seconds. The typical sample period is 0.012 seconds, corresponding to a sample rate of 83 Hz. This is very different to the expected 52 Hz.

data = numpy.genfromtxt("Device-2-Inertial.csv", delimiter=",", skip_header=1)

pyplot.plot(numpy.diff(data[:, 0] / 1E6))
pyplot.grid()
pyplot.show()

I suggest you close this issue and spend some time debugging and fixing the basics of your system.

[ladislas]

You appear to have made a mistake in the collection of Device-1-Inertial.csv because the first rotation occurs after 1 second, not 10 seconds.

I believe the scale of the plot is misleading. I thought you were right, but I have a 1e7 in the bottom right corner and when I hover with my mouse over the plot, I can see that that the x values are correct.

You must therefore be providing me with data that is different to what you are processing through the algorithm.

First I thought I was going crazy. But no, the data are correct, you need to change the sample rate in the script to 1/52

Then you'll get the right plot:

Device 1:

Device 2:

Device 2: (second try)

I used the following code to plot the difference between each timestamp in seconds. The typical sample period is 0.012 seconds, corresponding to a sample rate of 83 Hz. This is very different to the expected 52 Hz.

I noticed the same thing, I'm investigating. I'll close if nothing comes out of this.

[xioTechnologies]

The x axis is 1e7 because the units in the CSV file are microseconds but the script expects seconds. If you divide timestamp by 1e6 then the units will be seconds and you will have the same plot as me.

The 90° vs. 180° issue was my fault, I'm sorry. Processing Device-2-Inertial.csv using a deltaTime value of 1/52 provides the following plot. The y axis is rescaled in the lower plot. The error is <1° and not the ~18° you previously reported.

[ladislas]

First finding:

The typical sample period is 0.012 seconds, corresponding to a sample rate of 83 Hz. This is very different to the expected 52 Hz.

As x-IMU3 expect microseconds, I was creating my timestamps with a High Resolution Clock.
Turns out that the HRC implementation in mbed-os must be buggy, as time reported doesn't make any sense, i.e. ~20ms in normal clock ≡ ~12ms in HRC.

it's even worse than that: changing the ODR did not change the high resolution timestamp value, which made no sense. I'll report the issue to mbed-os.

With that fixed, timing is now accurate.

On one of our boards, rotation is also very accurate.

As for the other board, we are still missing ~20° but we suspect it might be a hardware issue.

I'll close this issue for now.

Thank you very much @xioTechnologies for the kind help :)