[bug] in Madgwick -> update_marg

[wangyendt]

// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrtf(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;

This is the code in void MadgwickAHRS::update of example_code/madgwick_mahony_c_code/MadgwickAHRS.cpp.
While refer to the raw paper of Madgwick Algorithm, I think bx and bz are supposed to be 2 times larger.

// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
bx = sqrtf(hx * hx + hy * hy);
bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_2bx = 2.0f * bx;
_2bz = 2.0f * bz;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;

[dlaidig]

That is quite interesting. The algorithm implementation is not written by me but by Sebastian Madgwick himself.

If I see it correctly at a first glance, the bug you are referring to is also present in the files downloadable at https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ ("C code (Header and source files)"). Is this correct?

Unfortunately, I probably won't be able to find time to dig through the whole algorithm in the near future. On a first glance, I agree that a factor of two seems to be missing in the code somewhere. Have you checked that this missing factor is not indirectly compensated somewhere else in the code?

[wangyendt]

If I see it correctly at a first glance, the bug you are referring to is also present in the files downloadable at https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ ("C code (Header and source files)"). Is this correct?

Yeah, the formula in the C code from this link and in this repo are the same.

At the beginning, I used the C code above and got unreasonable results using 9-axis Madgwick's algorithm. For example, when I set beta to 0.041, which is an official recommended param setting, the quaternion converges dramatically slowly. However, when I changed beta to 2.0, the output quaternion turns out to be reasonable.

So I compared the C code in this repo and a python lib here. I fed the same inputs and got different outputs (for 9-axis algorithm).
The python lib uses matrix form which is more comparable to the raw paper. The key difference is as below:
https://github.com/Mayitzin/ahrs/blob/9e02c5d8efe6c2fcb96ce6d4b0b20370d5d2c5f8/ahrs/filters/madgwick.py#L717

Then I did 2 things:

1. I used Mathematica to compare the matrix form of "J dot f" in the raw paper and the expanded form of "J dot f" in this repo, the formula turned out to be the same only if I double the value of bx and bz.
2. After I double the value of bx and bz, the outputs of the python lib and C code in this repo turn out to be consistant.

Anyway this implementation is out of date and no more maintained. XD

[dlaidig]

Just to see if this has an influence on the errors presented in the BROAD paper, I ran the evaluation code with your fix. And indeed, the error (TAGP) decreases slightly, from 4.96° to 4.69°. Fortunately, this just changes the numbers a little bit, but all comparisons and conclusions drawn in the paper remain valid.

The note about the x-io page no longer being maintained is quite recent. I am sure that many people still use this implementation and consider it to be the canonical version. So it is quite unfortunate that apparently there is a bug in this code and probably nobody will fix it. (Note that there is another version of the algorithm found in the appendix of the technical report. This version also handles the mag update slightly different.)

In this repository, I would like to keep the algorithm code in the original version, since the paper explicitly states that the implementation from the x-io website is used. I'll add a link to this issue in the README so that people using this implementation will be made aware.

Slightly off-topic: If you are interested in IMU orientation estimation, you might want to take a look at https://github.com/dlaidig/vqf.

[wangyendt]

Thanks for your great and valuable work on BROAD dataset and I am looking forward to studying your new work.