Multiple iterations of Madgwick = overshooting ?

[henzim]

This is not an issue with Kris' excellent code, but rather a question regarding the theory.

I kept getting bad yaw angles when rotating my setup (#182). This was probably due to (a) bad calibration and (b) the limited integration rate on a low-end 8MHz device.

I decided to implement Madgwick on the PC to verify this. I only rely on the device for raw values (acc, gyro, mag) and perform calibration myself. Already a first basic implementation (based on Kris' code and this works very well. The yaw angle still flickers (+/- 2°) but this is to be expected as mentioned by both Kris and the Madgwick paper.

The implementation. As soon as new data is available I call the Madgwick filter with the delta_tpassed since the last data (as in Kris' sketches)

I read in some other threads (e.g., #125, #175) that to get increased accuracy (convergence) one could/should perform multiple iterations (say 5-10) of the Madgwick filter for each new data.

This I do not understand.

To my understanding Madgwick updates the current quaternion by the gyro data (angular velocity [°/s]) intuitively like this (rough simplification): Qnew = Qold + delta_t * gyro So would not 2 iterations of this correspond to rotating the quaternion by twice the angle? At least this is what I am experiencing in my implementation, performing multiple (N) iterations makes the yaw angle "overshoot" its target approx N times, only to be slowly dragged back by the magnetometer one the movement is gone. (Disregarding the mag values (~MPU6050) simply leaves the final yaw angle at an N times too large value)

Does not the timestep delta_t somehow need be adapted?

The Madgwick paper describes finding an optimal time step to guarantee convergence, but then under certain assumptions (have not looked in depth at the details yet) it can be simplified (to how we use it). The question is if multiple iterations of this implementation with unchanged delta_t are legal?

//Henrik

[kriswiner]

The Madwick and Mahony algorithms are iterative since they use a steepest-descent like algorithm which asymptotes to a stable solution. So, generally one has to run them several times on a given set of data to converge to the "correct" answer. However, if you find good results after two iterations then you should use this.

On Sun, Sep 10, 2017 at 4:16 AM, henzim notifications@github.com wrote:

This is not an issue with Kris' excellent code, but rather a question regarding the theory.

I kept getting bad yaw angles when rotating my setup (#182) https://github.com/kriswiner/MPU9250/issues/182. This was probably due to (a) bad calibration and (b) the limited integration rate on a low-end 8MHz device.

I decided to implement Madgwick on the PC to verify this. I only rely on the device for raw values (acc, gyro, mag) and perform calibration myself. Already a first basic implementation (based on Kris' code and this http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ works very well. The yaw angle still flickers (+/- 2°) but this is to be expected as mentioned by both Kris and the Madgwick paper.

The implementation. As soon as new data is available I call the Madgwick filter with the delta_tpassed since the last data (as in Kris' sketches)

I read in some other threads (e.g., #125 https://github.com/kriswiner/MPU9250/issues/125, #175 https://github.com/kriswiner/MPU9250/issues/175) that to get increased accuracy (convergence) one could/should perform multiple iterations (say 5-10) of the Madgwick filter for each new data.

This I do not understand.

To my understanding Madgwick updates the current quaternion by the gyro data (angular velocity [°/s]) intuitively like this (rough simplification): Qnew = Qold + delta_t * gyro So would not 2 iterations of this correspond to rotating the quaternion by twice the angle? At least this is what I am experiencing in my implementation, performing multiple (N) iterations makes the yaw angle "overshoot" its target approx N times, only to be slowly dragged back by the magnetometer one the movement is gone. (Disregarding the mag values (~MPU6050) simply leaves the final yaw angle at an N times too large value)

Does not the timestep delta_t somehow need be adapted?

The Madgwick paper describes finding an optimal time step to guarantee convergence, but then under certain assumptions (have not looked in depth at the details yet) it can be simplified (to how we use it). The question is if multiple iterations of this implementation with unchanged delta_t are legal?

//Henrik

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/183, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qu5PhQAf0bm0JC24Htggmly8lMYLks5sg8T0gaJpZM4PSSNd .

[TediumRemedy]

Have you tried adjusting the input parameters for the algorithms (like, decreasing "beta" for Madgwick by the factor of two or so)? This may help with the accuracy with the price of the slower convergence speed. I tried calling Madgwick multiple times in a row with the same data - this did not increase the accuracy, it only made the loop rate drop on my MCU, since Madgwick is quite computationally intensive. (I used real delta_t for both calls separately, if that matters - i.e., calculated the time passed since the last call to the Madgwick routine)

[henzim]

Hi! Thanks for your reply. Yeah, I (just now, very roughly) tried playing around with beta, but that did not help. Using an adapted delta_t makes sense, I had no overshooting with using deltat/N when calling Madgwick N times in a row. But also no real improvement. I think the physical velocity given by the gyro [Deg/s] should just be multiplied once by the time deltat [s], to give the new angle ("quaternion") [Deg]. Multiplying twice (or more) would always mean going too far?! Of course the accuracy of a physical simulation depends on the frequency/time-step of the integration, but the current time-step must always match the current input-data?

This is what I still don't understand about the implementation (and unfortunately did not yet have enough time to look at the paper in detail), but to me it seems that the algorithm consists of two major parts: 1) a minimization of some functional (a "gradient descent correction step") and an 2) integration of the change over time, i.e.approx. (gyro*deltat) It is clear that optimization algorithms (such as gradient descent) in general should be iterated to achieve a good minimum. But since they have no separate inner loop of in the Madgwick function and both minimization and integration are always carried out together, I really don't see how it could work to call Madgwick several times with the same time-step (deltat).

Unless you can help me sort out my faulty view of the situation, I really must find the time to properly read the paper.... :)

thanks

[TediumRemedy]

Using an adapted delta_t makes sense, I had no overshooting with using deltat/N when calling Madgwick N times in a row. But also no real improvement. I think the physical velocity given by the gyro [Deg/s] should just be multiplied once by the time deltat [s], to give the new angle ("quaternion") [Deg]. Multiplying twice (or more) would always mean going too far?! Of course the accuracy of a physical simulation depends on the frequency/time-step of the integration, but the current time-step must always match the current input-data?

Yes, this makes sense for the integration step: (delta_t/2)gyro_angular_rate (first integration)+(delta_t/2)gyro_angular_rate (second integration), so you basically get your (delta_t*gyro_angular_rate), and integration is the same for 2 calls as it is for 1 call of MadgwickQuaternionUpdate, and you need to pass true delta_t. So the multiple calls that Kris recommends is supposed to influence only for the minimization step, but for me it doesn't. Here's what I am doing though: I ask the sensor if it has any data available, and if it doesn't, I just call MadgwickQuaternionUpdate, with the appropriate delta_t, then repeat it in a loop and see if new data has come. So it may be called with old gyro/accelerometer/mag data several times.

If the accuracy is really bad, perhaps you should look at calibration again? it looks like sometimes hard-iron calibration of the magnetometer is not enough and you need soft-iron calibration using a rotation matrix and then scaling the axes (I have had a rotated ellipse, after I mounted a large piece of metal near the MPU board). Also perhaps gyro and accelerometer are biased and need to be "zero-centered"? Do you have vibrations (and if so, you need to set up low pass filters). My board returns pretty accurate data though, only magnetometer calibration was necessary...

[henzim]

Hey that sounds like a great idea, makes sense to me. Instead of postponing the next integration for "too" long and then suddenly accrediting all the time past to the new measurement, the past time is accredited to the last seen measurement until a new one is seen. But possibly that does not even make a difference, but you do obtain a higher calling rate of the madgwick function with a correct time step, perhaps that is beneficial, even if the gyro data does not change. Will hopefully be able to experiment more with it this weekend. Thanks again for you input.

What is your setup by the way? Arduino Mini Pro 8Mhz? And just to be sure what order/which signs of parameters do you call Madgwick?

[henzim]

Now that I think about it, that is actually how Kris' sketch and mine work already. (For a moment i thought I always waited until new data was available) Now I am no longer sure how to add more iterations. Possibly it might also be a bad idea to perform too many Madgwick iterations if it keeps/blocks you from receiving new data...

[TediumRemedy]

STM32F103/72MHz MadgwickQuaternionUpdate(-a_x, a_y, a_z, g_x, -g_y, -g_z, m_y, -m_x, m_z);