Scale problem with Madgwick

Mayitzin / ahrs

Attitude and Heading Reference Systems in Python

https://ahrs.readthedocs.io/

MIT License

557 stars 88 forks source link

Scale problem with Madgwick #29

Closed anisghaoui closed 3 years ago

anisghaoui commented 3 years ago

Edit : this issue is irrelevant, I had a data rate of 400 hz, not 200 hz. Lesson : check your data first, don't trust the provider.

Hi, I have made a test run with Madgwick's filter and I obtained results that are twice as big as they should be. hear me out:

my sampling frequency is 200 hz, default is 100. I made 90° rotation but Madwick predicts 180° instead.
Going here and dividing it by 2 solves the problem for me. https://github.com/Mayitzin/ahrs/blob/b98aa3dd0513a52261b2e0cf06b52a7e0693722a/ahrs/filters/madgwick.py#L600

Mayitzin commented 3 years ago

Hi, did you try to set the frequency at the creation of the Madgwick object?

madgwick = ahrs.filters.Madgwick(frequency=200.0)

anisghaoui commented 3 years ago

yes I did like this, my_mag = Madgwick(acc=acceleration, gyr=gyroscope, frequency=_frequency) I debugged it and it seems to build the object correctly

Mayitzin commented 3 years ago

I cannot reproduce the same results. I've tested with a couple of data files and it seems to be working on my side. When I divide that part as suggested, the results worsen.

It'd be worth checking:

The angular rates are given as radians/second.
The filter's gain is set to a proper value. Default is 0.033 when using Gyroscopes and Accelerometers; and 0.041 when also using Magnetometer. When the gain is equal to zero, it will discard all measurements except for the Gyroscope.
The sampling frequency is uniform (does not vary from 200 Hz.)

For reference, I test with the recordings of the RepoIMU dataset described in the Discussions: https://github.com/Mayitzin/ahrs/discussions/20#discussioncomment-260648

anisghaoui commented 3 years ago

Hi again, Thanks for the feedback. I will recheck my data as I am having weird estimates from the Madgwick filter on the z axis. Please don't close the issue yet. Will be back as soon as I have news.

anisghaoui commented 3 years ago

Hi, I have indeed checked all these points and they are coherent and correct.

I have a little question. Right now, i used your to_angles() implementation in Quaternion. Do you ensure compatibility between Madgwick.Q attribute and the constructor of Quaternion() then to_angles() method ? I am doing this right now

madgwick = Madgwick(acc=acceleration, gyr=gyroscope, frequency=frequency)
# madgwick.Q contains an iterable N,4 shaped np array.
# build a quaternion q convert it to angles 
orientation = np.array(list(map(lambda q: Quaternion(q).to_angles(), madgwick.Q)))

edit : typo

anisghaoui commented 3 years ago

Hi again, I just updated all the package. Instead of cloning the repo, I got it with pip. thanks for the release. The scale problem isn't related to frequency. I rechecked your quaternion to angles conversion, it is exactly as described in wikipedias and corresponds to what Sebastian Madgwick himself wrote in his C code.

I am having weird behaviours where the filter can estimate correctly X axis but has trouble estimating Y or Z. I am closing this issue as I will be soon opening a new one about that. Be back soon. Thank you.