Sensor Fusion Algorithm

daylanKifky

hi, it's not easy to tell where the problem is coming from. you should get some decent results leaving all those constants at their default values.

Just to doublecheck.. how are you inputting the raw sensor values to the algorithm? I don't know if you noticed that the LSM9DS1 measures the values with three different reference systems, one for each sensor,

from the datasheet, page 10

The kalman filter of course expects all the values to be expressed in the same reference system.
What we do is convert all of them to a right handed, Zup up like the one Blender uses. In order to do that we map the raw values like this:

kalman.set_gyro(imu->gy, imu->gx, imu->gz);
kalman.set_acel(-imu->ay, -imu->ax, -imu->az);
kalman.set_mag(imu->my, -imu->mx, imu->mz);

Note: the accel reference system might seem equal to the gyro one, but you are getting the inertial or Fictitious force which points in the opposite direction from the actual acceleration vector. More info in this article

mason_safavy

daylanKifky
Hi again,
That was a interesting information about opposite direction of accelerometer and gyroscope.
I used the exact order you mentioned but unfortunately the results are the same I also tried to opposite the gyro values in my own implemented order, didn't work neither.
Here is a interesting thing, the same problem goes for mahony and madgwick. (after a fast rotation it takes some time to update the new state)
I'm really confused and I don't know what is causing all these.

Regards
Mason

daylanKifky

Well, this is getting trickier. But the fact that you get wrong results with the other estimation algorithms suggests there's a problem with the sensor itself or the way the raw data is handled

perhaps take a look at the raw data coming from the sensor trying to spot some inconsistency or problem, for example:

Sometimes a sensor get's broken and it outputs 0, 0xFFFF or something similar as one of it's outputs
doublecheck you are getting values that are consistent with the precision you set. For example, when the sensor lies flat on the table you should get an uint16 value which divided by the precision should give you ~1g in Z. you can do the same for the other axis by rotating the sensor. The gyro and mag are trickier to test like this, but if you get creative you might find a way, otherwise let me know

mason_safavy

daylanKifky
I will check all of these, but there is something that I think it's causing the problem.
I think it's coming from the sensor library, I will try to read the sensor values directly without library just using Wire library with 400 KHz Freq.
I'm gonna let you for any progress.
Regards

daylanKifky

mason_safavy
that sounds like a good idea too. let me know how it turns out

mason_safavy

daylanKifky
Hi Bruno, I hope you're well.
After wasting some time writing scripting for reading sensor values with the wire library and finding out Kriswiner has done it already, link: https://github.com/kriswiner/LSM9DS1
I finally tested the 400khz wire clock with the following settings.

uint8_t Gscale = GFS_2000DPS;// gyro full scale
uint8_t Godr = GODR_119Hz;   // gyro data sample rate
uint8_t Gbw = GBW_med;       // gyro data bandwidth
uint8_t Ascale = AFS_4G;     // accel full scale
uint8_t Aodr = AODR_119Hz;   // accel data sample rate
uint8_t Abw = ABW_50Hz;      // accel data bandwidth
uint8_t Mscale = MFS_4G;     // mag full scale
uint8_t Modr = MODR_80Hz;    // mag data sample rate
uint8_t Mmode = MMode_HighPerformance;  // magnetometer operation mode
Kalman Filter Frequency = 450 Hz -->  Filter.update(450);

The results are far better and I can say it's okay when I'm moving the sensor but still I have some problems with leaving the sensor steel on the desk, Unfortunately I have some yaw drift but it corrects itself quickly. For example it drifts 2-3 degrees and corrects exactly with the same values, whole drift and correction process takes about 6-10 seconds. I think it's related to IMU calibration, but I don't know which exact sensor is causing the problem, I think it should be Magnetometer or Gyroscope.
I forgot to say when I'm using the data order you sent, all these problem are kind of okay but I do have some noisy output with a little bit of yaw drift which leads to wrong provided data order. Actually the only problem I have after using your data order is yaw drift.
I just wanted to let you know where I am.
Also I'm grateful for all your helps, even adafruit's community is not responding. I thinks this is the difference between open source communities and other ones. I appreciate.
Best Regards.
Mason

daylanKifky

Hello! yes you need to perform a calibration to the sensors.

For the gyro and accel take some samples while the sensor is laying flat on a table, average them and you have your correction vector. Remember to subtract 1g from the axis which is normal to the ground (normally Z).

Then subtract the vector to the readings before feeding them into the kalman filter.

The mag calibration is more complicated. The original NXP Kalman filter had a live mag correction step, but the Adafruit one is strongly modified and it seems like they took it away. Instead they provide an example calibration.ino where they use another Adafruit library. I guess it will also take care of calibrating gyro and accel

mason_safavy

daylanKifky
Hello Bruno
Thanks again for responding,
After 2 weeks of testing the library and doing some good calibration,
Finally I found out the covariance matrices variables are causing the yaw drift.
As you are the only man who is helping so I decided to ask for help again.

these variables are killing me and my project, I will be grateful for anyhelp.
Kind Regards,
Mason

daylanKifky

Hi!
As I said, the adafruit library is heavily reworked, and many of these constants are not present in the original one (they are probably calculated somewhere else)

But in my experience it doesn't require tweaking of these types of values to get a decent result at the beginning. Instead I would double check that:

You are inputting the values of the Sensor in the correct order (I described the reordering from the LSM9DS1) in an answer above)
You are setting the correct sample rate to the Filter
The 3 sensors are properly calibrated and the calibration is being applied. In order to check this a good idea is to make a dump of the readings in CSV, and then plot the results, you should see:
- The calibrated gyro readings are ~ 0 + small noise when the sensor is static
- The calibrated accel readings are ~ 0 + small noise when the sensor is static, except for one axis that should be ~ 1g + noise. Try different orientations and check that the 1g axis changes accordingly
- The mag requires a little more work: you should move it around in all dimensions while you dump the values, then make a 3D scatterplot: you should see a sphere centered at the origin of the world

mason_safavy

daylanKifky
Thanks again for your attention.
I'm working on the covariance variables, all the problems are related to these variables, so far it going well for me.
I'm gonna let you know what exact setting should be used.

Regards
Mason

daylanKifky

Great!
Keep us informed, would love to know the setting that works for you!

mason_safavy

daylanKifky
Hi again,
After an incredible journey, finally I have acceptable output.
After realizing that the adafriut AHRS library is written for FXAS21002C, I started compare the noise values of LSM9DS1 and FXAS21002C. It's just 10 times the FXAS21002C, not that much 🙂
and multiplying the gyro values by 10, the results are kind of acceptable but it can be better.
I just wanna thank chordata and your community for supporting an outsider.

Best Regards.
Mason

mason_safavy

Hey Bruno,
I wanna appreciate all the previous things that you have done, and another huge appreciation for the possibility that you're making use only of the software side Chordata.
We're working on an open source project like you, and I'm just an honored member.
Wait for an introduction video from us which will be special for you.
can't wait to get my hands to the python script.
I forgot to say I'm a guy. 😆

Best Regards,
Mason

daylanKifky

Hello mason_safavy!
You are very welcome! and can't wait to see the intro video, thanks!

Once the python module is released we would really appreciate your feedback

daylanKifky

By the way if anyone is joining the discussion at this point: we talked about it and some developments related to it in this livestream. In particular we are quite exited about the possible applications of the new notochord as python module which will expose (among many other things) python endpoints to the Kalman filter we were talking about with @mason_safavy

« Previous Page