Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Help with very slow convergence time in Madwick / Mahony filters. #363

Open
cwarrior777 opened this issue May 4, 2019 · 15 comments
Open

Comments

@cwarrior777
Copy link

Hi Kris, this is the second time I am writing to you, the truth is that your code is great!
Beyond that, I am having problems in the convergence of the data when using the Madgwick and Mahony Filters.
I tell you that I am using the code MPU9250_MS5611_BasicAHRS_t3.ino which I think is the most complete.
Regarding the MCU that I am using, I started testing this code in an arduino Mega and it worked really well, with slow but acceptable changes when turning the sensor in different positions, but the reality is that I must develop this in an STM32F429 MCU, which is much more powerful (180MHz Clock) and even has FPU.
The problem is that when passing the code to this new MCU (in an IDE language C, clarified I had to convert your quaternion libraries to new functions for this ide, for which, I pass the same data as you do in your code, but I also add the integration time and the beta in the data that happened to the function), the convergence time is REALLY SLOW, I'm talking about 60 seconds to reach the angle of change, which is impossible since I must use this sensor for aeronautics with relatively fast and stable changes of 2-3 ° deviation.
To comment a bit on the parameters that happened to this function and making a parallel:
MagdwickQuaternionUpdate (-ax, -ay, -az, gx * PI / 180.0f, g and * PI / 180.0f, gz * PI / 180.0f, my, mx, -mz); It is the function of your ide.
And the one that I converted to C:
AHRSIMU_UPDATE_MADGWICK (AHRSIMU_t * AHRSIMU, float ax, float a, float az, float gx, float g, float gz, float mx, float my, float mz)
AHRSIMU_UPDATE_MAHONY (AHRSIMU_t * AHRSIMU, float ax, float a, float az, float gx, float g, float gz, float mx, float my, float mz)
Where AHRSIMU is the entity where I keep the quaternions and the Yaw Pitch and Roll angles, this entity also has inside deltat and beta that will be used.
Unlike in your program I have fixed a fixed deltat value of 0.0039 which when I try to decrease it makes the convergence time even worse, the beta that I used is 0.6 although I tried to use a beta of 0.041 (with results still slower).

I hope you can read all this and help me since I can not fully understand the mechanisms of step of integration and beta of the fusion filters I feel that I am not taking advantage of the processing power of my MCU.

I thank you, Greetings!

@kriswiner
Copy link
Owner

kriswiner commented May 4, 2019 via email

@cwarrior777
Copy link
Author

Hello Kris,
No, I was wrong when I passed the form of the filter statement, the way I pass the data to the filter is this:

void NR_AHRSIMU_UPD_MADGWICK(NR_AHRSIMU_t* AHRSIMU, float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)

With respect to the orientation of the sensor, it is in NED, only I have not commented that for welding reasons I decided to use the sensor turned as seen in the photo, this is how we see it from above:

748115-mla25185591723_112016-o-cbeb4f24cacac72e3815575061329045-640-0

Therefore, as you see in these diagrams, I have solved the orientations in the following way (first image), according to the mpu9250 data sheet (second image).

NED orientation

original orientation

Ok, I know that although the delta is not arbitrary, I really do not know how to develop a variable delta in our IDE (IAR Systems). Using a for and repetitive iterations of the Madwick function (around 200 times with a delta of 0.002) I have achieved a fast data convergence towards the angle in which I move it, otherwise when iterating only one or a few times the filter, I get a really slow convergence response.

The last problem that I am having is the little stability that there is around the values ​​of YAW, it has a variation of + -6 degrees (which is unacceptable), I do not know if this is a product of the beta that I am using ( beta = 0.041) is low or if I have to calibrate the system even better, then I attach the images obtained from the measurements of the fields Mx, My and Mz:

normal sampling

total sampling

Finally, underneath, read some values ​​of the ROLL PITCH and ROLL angles at rest so that you can see specifically the variation of the YAW without movement:

ROLL PITCH YAW
-2.841019 1.909491 0.677246
-3.092792 1.935598 358.830597
-2.583346 1.903627 0.449164
-2.762627 1.927869 1.028311
-3.495186 1.939279 359.242554
-2.819450 1.924049 358.417236
-3.109864 1.928123 356.571136
-2.080033 1.920668 358.091034
-1.845699 1.932480 359.886566
-1.544893 1.913306 1.699031
-2.782847 1.886256 2.664880
-2.234194 1.890347 4.364385
-2.893011 1.980402 2.655517
-3.038342 1.929583 0.718986
-2.279603 1.934345 2.363921
-2.786048 2.046722 2.498289
-2.580220 1.944912 4.235439
-1.977362 1.861373 5.941456
-3.053561 1.978192 4.347548
-3.815175 1.971648 2.546618
-2.822617 1.996431 2.011711
-2.592336 1.913283 3.704556
-2.778693 1.905831 3.518666
-2.724808 1.967276 2.065395
-2.690906 2.000078 1.884261
-2.491514 1.909421 3.671382
-3.575563 1.858284 2.056322
-3.709345 1.931224 0.107186
-2.332744 2.076974 1.470321
-2.933634 1.795029 1.972743
-2.986170 1.943429 3.023360
-3.740069 1.880241 1.207008
-2.657408 1.898249 2.603940
-2.862254 2.028811 2.929603
-3.298363 2.003579 1.069905
-3.278937 2.028494 359.162933
-2.656162 1.975354 0.721982
-2.708680 1.909566 0.159184
-1.924360 1.907970 1.811797
-2.758598 1.982924 3.337498
-3.636888 1.946851 1.586542
-3.217878 1.911505 359.739075
-2.842207 1.897478 357.914551
-3.084610 1.924206 356.113403
-2.382006 1.964931 357.743134
-2.171737 1.969217 359.536285
-2.487619 1.800841 1.282838
-2.172706 1.967942 3.036295
-3.218053 1.984018 1.322450
-2.997139 1.883619 359.433380
-2.799490 1.854677 0.995750
-2.920923 1.843165 2.718146
-2.066543 1.866907 4.344150
-2.880691 1.872111 3.034091
-2.980692 1.957958 1.233219
-2.762504 1.966444 1.167916
-2.929309 2.013844 2.734053
-3.423189 1.977945 0.862212
-2.937435 2.015821 1.340721
-3.556823 1.928998 359.516602
-3.491372 1.861911 357.567780
-2.384296 1.936852 359.025635
-2.938719 1.898175 359.701691
-2.899185 1.999137 357.811798
-2.263216 2.007485 359.520569
-2.570843 2.013480 1.282870
-2.726504 1.972003 1.792309
-2.481748 2.027904 3.566798
-3.143746 1.909043 1.809715
-2.843090 2.102052 3.100708
-2.916345 1.992654 3.674736
-3.598850 2.004102 1.797361
-2.341906 1.920233 3.184055
-2.518298 1.972791 4.964610
-2.866606 1.975747 4.246680

I hope you can help me, greetings!

@kriswiner
Copy link
Owner

kriswiner commented Jun 12, 2019 via email

@cwarrior777
Copy link
Author

Ok Kris, again I apologize because I have mistakenly copied the header of the function that I usually use (I copied the header of the function statement instead of the call to it), the function I call receives the following values :

NR_AHRSIMU_UPD_MADGWICK(&AHRSIMU_MG, -(MPU9250.Ax), -(MPU9250.Ay), -(MPU9250.Az), MPU9250.Gx, MPU9250.Gy, MPU9250.Gz, MPU9250.My, MPU9250.Mx, -(MPU9250.Mz))

(Please do not pay attention since MPU9250 is the body of the entity that I use to store the data and AHRSIMU_MG is the entity where I keep the YPR values ​​obtained).
Also here below I clarify the point of north orientation on the image that I have uploaded previously.

748115-mla25185591723_112016-o-cbeb4f24cacac72e3815575061329045-640-0

Currently I am calling the Magdwick function outside of the detection interruption of a new data, therefore the program will iterate the existing data even if a new measurement has not arrived.
The current low-pass filter bandwidth and sampling values ​​are:
-> Accelerometer and Gyroscope: (Fs = 1 KHz) DLPF = 41Hz and Sampling Rate = 200Hz
-> Magnetometer: Continuous sampling at 100 Hz
My query is, how could I get the sampling frequency values ​​you suggest?
Would it be more convenient to raise Fs and maintain the value in SMPLRT_DIV or maintain Fs and vary the latter ?.
I make the same query for the filter, since the closest value to which you suggest is 92 Hz for the accelerometer and gyroscope or if the next one is 184 Hz.
I'm going to test the sampling and low-pass filter frequencies you suggest to see how they turn out.

I await your response regarding these configurations.
Thank you!!

@kriswiner
Copy link
Owner

kriswiner commented Jun 14, 2019 via email

@cwarrior777
Copy link
Author

You may not have understood me because, when I refer to Fs, I'm talking about the frequency with which I get the data from each sensor, and when I speak of sampling rate I mean the SampleRateDiv.
My question is, when you suggest me to vary those frequency values, do you mean the frequency Fs of the sensors or the final frequency SampleRateDiv?

@kriswiner
Copy link
Owner

kriswiner commented Jun 14, 2019 via email

@cwarrior777
Copy link
Author

1066/5000
Hello Kris,

After a few weeks of testing, the suggestions they have given me have managed to make the fusion filter work (either with Magdwick or Mahonny) with a quite high update rate (around 60 KHz) and a fast convergence of the values ​​of angle.
The problem I have is in the variation (or error) of the angle YAW, it has a variation of + -2 degrees after converging, which vary permanently second by second, so it is unacceptable to use these readings. The strange thing is that the Pitch and Roll angles remain VERY constant, so I do not know how I can solve this issue, I need to have stable YAW angles or at least with a minimum error of 1 °, how would you suggest I can improve this? ?
On the other hand, I'm looking for the best Kp and Ki coefficients for the Mahonny filter, have you used that filter? I have seen from your program in arduino that you have used only the coefficient Kp = 10 but without coefficient Ki, what values ​​would you suggest for the latter?

I appreciate your time Kris! Regards!

@kriswiner
Copy link
Owner

kriswiner commented Jul 10, 2019 via email

@cwarrior777
Copy link
Author

If Kris, I understand what you say, the question is, what is the optimal way to calibrate the magnetometer ?, since at the moment it is even happening to me that when I compare with a compass in the angle 0, in my sensor I get a measurement of more that ~ 6 ° (when the sensor is still), I suppose this is due to a lack of fine calibration in the magnetometer (not in the accelerometer and gyroscope).
So if you can, I ask you to explain what is the best way to calibrate this or at least show me the best post you've seen about this.
Thanks and regards!.

@kriswiner
Copy link
Owner

kriswiner commented Jul 10, 2019 via email

@cwarrior777
Copy link
Author

Yes, I've seen it, it's a very good post, and that's what I'm basing myself on, graphing in Matlab the results of measuring the axes of the magnetometer to graph the sphere.
But the resulting spheres are from -200 to 200 and still I do not achieve a perfect circular shape besides small points outside the circular sphere as you see in the image.

TEST_MAG_0507_2_MXY
TEST_MAG_0507_2_MXZ
TEST_MAG_0507_2_MYZ
TEST_MAG_0507_2_COMPLETO

I made the query to you in order to know if there is any correct way to do the "shape of eight" to calibrate it.

@kriswiner
Copy link
Owner

kriswiner commented Jul 11, 2019 via email

@cwarrior777
Copy link
Author

Sorry but, what do you mean when you talk about freescale solver? Its a software?

@kriswiner
Copy link
Owner

kriswiner commented Jul 17, 2019 via email

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants