comparizon of Madgwick, Mahony and EKF filters

geoKinga commented 3 years ago

Hi I have recorded data from my mobile phone (accelerometer, gyroscope, magnetometer). Then I used ahrs library to calculate attitude parameters (quaternions and then conversion to yaw, pitch, roll).

The results for Madgwick and Mahony filters are quite similar but the results of EKF totally differ.

Could you please give me some tip what could be a reason for these different results for EKF filter (I have to use the same data for all filters)

staly_0 1_0 1_0 7

moutayam commented 3 years ago

Could you please elaborate more on in what conditions you recorded the data? Static position, moving, etc.

gabrielecoppi commented 3 years ago

I got the same results using the example file in the tests folder. data = ahrs.utils.io.load("repoIMU.csv") orientation = ahrs.filters.ekf.EKF(data.gyr, data.acc, data.mag) q_ekf = orientation.Q orientation = ahrs.filters.mahony.Mahony(data.gyr, data.acc, data.mag) q_mahony= orientation.Q and I get different results after few samples, here you can see the difference in the the different quaternion components ahrs

Mayitzin commented 3 years ago

Hi everyone, what I can see, the initial values might be off.

Quaternions must have a Euclidean norm is equal to 1 (a.k.a. versors) to be used as orientations (or rotation operators).

What I can see is that all initial values are equal to zero, which would give you a norm equal to 0. That is not a valid versor and, therefore, does not represent any orientation.

Even worse, starting with an invalid quaternion would yield a set of invalid orientations, as explained in #11 .

It is recommended to give a valid initial quaternion for algorithms using the gyroscope to integrate the angular rate (Madgwick, Mahony, EKF, etc.) Many use the common quaternion identity [1, 0, 0, 0], (whose norm is visibly equal to 1), but you're also welcome to use a static attitude estimator like (Tilt, TRIAD, Davenport's, etc.)

In the meantime, I will try to add some warnings of invalid orientations to save us the future headaches.

gabrielecoppi commented 3 years ago

@Mayitzin thanks for the answer. I rechecked the script and effectively I was doing something wrong. The new script gives me these results. I am still using the repoIMU.csv file that you uploaded in the test folder. As you can see now the norm is always 1 but the results are significantly different, especially for the EKF
ahrs

Mayitzin commented 3 years ago

There are two points to highlight:

The negative of a quaternion represents the same orientation. For example, if we have the quaternion [0.18, 0.37, 0.55, 0.72], its inverted quaternion [-0.18, -0.37, -0.55, -0.72] would represent the exact same orientation. So, if lines q0, q1, q2 and q3 flip with respect to another estimation, it is still ok. They are actually the same orientation.
The EKF requires a set of noise variances, which define the certainty of each sensor. By default, they are defined as 0.3^2 for the gyroscopes, 0.5^2 for the accelerometers, and 0.8^2 for the magnetometer. The smaller they are, the more you "trust" the sensor. Try redefining them when you create the class. You can do that by passing a 3-element array to the parameters noises:

ekf = EKF(noises=[0.1**2, 0.5**2, 0.8**2]) # Here I set gyroscope to 0.1^2

Unfortunately, there isn't a standard noise variance value for each type of sensor. Those values have to be determined for your own precise model and design. There are certain techniques that would help you to estimate these variances, but that is out of the scope. Here you can try and tweak the noises to see what works for you.

I will directly mention this in the documentation of the EKF (working on it.) Thanks for pointing it out!

gabrielecoppi commented 3 years ago

Thanks, I will try to tweak the noises in the EKF filter to get something closer to the others. Since it is the file in the repo, do you have noise values that can be used?

Mayitzin commented 3 years ago

I've not tested enough with all the values, but I think the noises [0.02**2, 0.5**2, 0.9**2] work quite well. A Gradient Descent Algorithm could be handy here, if anybody is up for playing a bit further ;)

ALAXAY commented 3 years ago

hello, I'm trying to figure out how to build graphs and I get an error

  1 import ahrs
  2 data = loadmat("ExampleData.mat")

----> 3 ahrs.utils.plot_sensors(data.gyr, data.acc, data.mag, 4 x_axis=data.time, subtitles=["Gyroscopes", "Accelerometers", "Magnetometers"])

AttributeError: module 'ahrs.utils' has no attribute 'plot_sensors'

Mayitzin commented 3 years ago

hello, I'm trying to figure out how to build graphs and I get an error
  1 import ahrs
  2 data = loadmat("ExampleData.mat")
----> 3 ahrs.utils.plot_sensors(data.gyr, data.acc, data.mag, 4 x_axis=data.time, subtitles=["Gyroscopes", "Accelerometers", "Magnetometers"])

AttributeError: module 'ahrs.utils' has no attribute 'plot_sensors'

Which version are you using? The plotting functions are no longer available importing the newest versions.

ALAXAY commented 3 years ago

I am using the latest version, which I downloaded from the git hub, please tell me how to build graphs, very grateful for the answer.

пт, 8 янв. 2021 г. в 13:49, Mario Garcia notifications@github.com:

hello, I'm trying to figure out how to build graphs and I get an error

1 import ahrs 2 data = loadmat("ExampleData.mat")

----> 3 ahrs.utils.plot_sensors(data.gyr, data.acc, data.mag, 4 x_axis=data.time, subtitles=["Gyroscopes", "Accelerometers", "Magnetometers"])

AttributeError: module 'ahrs.utils' has no attribute 'plot_sensors'

Which version are you using? The plotting functions are no longer available importing the newest versions.

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/Mayitzin/ahrs/issues/12#issuecomment-756715117, or unsubscribe https://github.com/notifications/unsubscribe-auth/AIHPV422XI67QPSQCIXIDCDSY3WOFANCNFSM4UPUPOJA .

Mayitzin commented 3 years ago

Hi,

sorry for the late reply. As plotting is being removed from the package, I would suggest to use matplotlib for your project. It is the most common plotting package for Python, with plenty of support from the community and even more resources to create nice visualizations of it.

For a simple visualization of an N-by-3 array with sensor data you would simply do:

import matplotlib.pyplot as plt
plt(data[:, 0], 'r-')
plt(data[:, 1], 'g-')
plt(data[:, 2], 'b-')
plt.show()

which would open a window showing the X-, Y- and Z-axis data with red, green and blue lines, respectively. It is that simple. Further customization can be done with few parameters passed to these functions.

Mayitzin / ahrs

comparizon of Madgwick, Mahony and EKF filters #12