xiotechnologies / open-source-ahrs-with-x-imu

Open-Source-AHRS-With-x-IMU

Warning

This repository is no longer maintained and has been superseded by Fusion.

More information available on website.

open-source-ahrs-with-x-imu's Issues

how to set the argument Beta and sampleperiod?

Thank you for your implementation of this algorithm, but i do not know how to set the two argument:beta and sampleperiod of MadgwickAHRS.cs, can you please help?

coefficient 2 missing

Hello, I'm checking the formulas used in the code. I can understand all formulas apart from one, which I think might be wrong in your code.

In file MadgwickAHRS.cs lines 150-151 you have:

            _2bx = (float)Math.Sqrt(hx * hx + hy * hy);
            _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;

but I think you should have a multiplicative 2 coefficient:

            _2bx = 2.0f * (float)Math.Sqrt(hx * hx + hy * hy);
            _2bz = 2.0f * (-_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4);

The vector [mx, my, mz] is the magnetometer output. Such vector is renormalized. Then the vector [hx, hy, hz] is computed using quaternion rotation:

            hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
            hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
            hz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4

(hz is equal to _2bz in the code). One can check that the above formulas are correct and correspond to the rotation given by the quaternion [q1, q2, q3, q4] which is normalized. Hence the resulting vector [hx, hy, hz] is a unit vector. If you let bx = _2bx/2 and bz = _2bz/2 we expect that the vector [bx 0 bz] should be a unit vector too. This is suggested by the name, but is also confirmed in the code which follows. In the computation of [s1, s2, s3, s4] (at lines 156-159) the coefficents _2bx and _2bz are used. The vector [s1, s2, s3, s4] is the gradient of the function f described in Madgwick article. The objective function f_b evaluates the difference between the earth magnetic field and the field obtained rotating [mx, my, mz] in earth coordinates. Actually [hx, hy, hz] are the coordinates of [mx, my, mz] in the earth frame and [bx, 0, bz] should be the vector with same inclination of [hx, hy, hz] unit norm and north direction. The objective function should evaluate the difference between [bx, 0, bz] and [hx, hy, hz]. I checked the the gradient [s1, s2, s3, s4] of the objective function is correct if we assume that _2bx and _2bz represent 2*bx and 2*bz as the name suggest.

You can check the code by printing hx*hx+hy*hy+hz*hz and _2bx*_2bx + _2bz*_2bz and see that both are close to 1.0 while the second one should be close to 4.0 since [_2bx/2, 0, _2bz/2] should have length 1.

License?

Please select an open source license when releasing software to the public so people know if they can use it or not.

Placement of magnetometer w.r.t accelero and gyro?

Hi,

Can you please let me know the placement of magnetometer wrt to accelerometer and gyrometer as in datasheet axes direction of acc and gyro are given but that of magnetometer are not there.