Madgwick begin( )

[PaulRowntree]

I am using the NXP 9dof IMU board, and it seems to work well, calibrates well. The Yaw angles returned from the Madgwick AHRS codes seem to vary inversely with normal compass angles, but that was an easy fix.

I am wondering about the parameter in the madgwick begin( ) statement ... the class code describes it as the SampleRate, and the example code says the code 'expects' 70/second, but the downloaded code uses begin(10). If I change this to begin(70) the various output angles converge slowly to their values as I move the board, with a time constant of ~10 sec. begin(10) responds quite quickly. begin(1) is unsteady, and the roll oscillates by 10 degrees between outputs.

Can anybody explain this parameter for me please? Or should we treat it as a user setting ... tweak for best results?

Thanks!

[adafruit_support_mike]

The whole function is actually a one-liner in the header file:

Code: Select all

    void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }

The value 'invSampleFreq' is the period between readings, and is used to integrate the motion and position vectors. You calculate the incremental change in velocity by multiplying the acceleration (read from the sensor) by the amount of time since the last reading. In a similar way, the incremental change in position is the velocity multiplied by the amount of time since the last reading.

You have to make that value match your update rate for the numbers to make sense.

[PaulRowntree]

Hi Mike, thanks!
I saw that in the .h, and invSampleFreq is used to scale the integration as you say.
The ms timer says that each iteration of the update is ~15ms, which is about 60 Hz. But if I use begin(60) the output responds very very slowly (~10 sec to even approach the final orientation after a quick motion). This seems odd. What am I misunderstanding here?

Is it possible the NXP gyro scales have changed between the calibration and the use stages?

[PaulRowntree]

I think I have it ...
The Madgwick paper used a 512 Hz sample rate, and optimized the gain (beta) at 0.1 (some of his graphs look like 0.04, but there may be a hidden factor in there).
If we treat the parameter in the begin( ) as the true sample rate (in Hz), then we need to adapt the value of beta to make the loop responsive enough. With ~60 Hz sample rate, beta seems to be ~1.0, not 0.1). If beta increases too much beyond the optimum, the pitch/roll values become unstable. It looks like (beta*sampleFreq)~=50 to 60.

I added a line to Madgwick.h to set beta on the fly.

Code: Select all

public:
    Madgwick(void);
    void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
	
    void setBeta( float newBeta ) { beta = newBeta; }        // code added by pr Nov 2017
	
    void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
    void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);

Cheers!

[adafruit_support_mike]

Yeah, there’s some interaction between the factors.

Modern control systems work on the assumption that you can crunch numbers much faster than the system you want to control. The full versions (Kalman filter) use the previous settings to predict the next reading, compare that prediction with the actual reading, run a statistical analysis to keep track of the average error in the predictions, and tune themselves to minimize the error.

The Madgwick filter skips the prediction and analysis, and just hard-codes the ratio of previous data to raw input it will use for the next prediction. For systems where the large-scale system parameters don’t change much, it works almost as well, and is a heck of a lot easier.

I haven’t read the AHRS code in detail, but beta is probably the combination ratio. Higher ratios make the output respond to real changes faster, but also increase the output noise and tendency to overshoot. Lower ratios make the output smoother, but more sluggish responding to real changes.

The trick is to find the value that gives you the combination of speed and smoothness you like best.