Madgwick AHRS algorithm Android implementation

Question

I'm trying to implement Madgwick sensor fusion algorithm from here, to calculate orientation of my phone. It works relatively fine when I am moving, but it if the phone is steady on the table it starts to accumulate errors very quickly. I tried to play with sensor update rate and filter gain, but it didn't help

I also tried to edit the code according to this solution, but it also didn't work. Is there a way how to fix this? Thanks

MadgwickAHRS algorithm:

 public class MadgwickAHRS
{
    public float SamplePeriod ;
    public float Beta;
    public float[] Quaternion;

    public MadgwickAHRS(float samplePeriod)
    {
        SamplePeriod = samplePeriod;
        Beta = 1f;
        Quaternion = new float[] { 1f, 0f, 0f, 0f };
    }

    public MadgwickAHRS(float samplePeriod, float beta)
    {
        SamplePeriod = samplePeriod;
        Beta = beta;
        Quaternion = new float[] { 1f, 0f, 0f, 0f };
    }

    public void Update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
    {
        float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3];   // short name local variable for readability
        float norm;
        float hx, hy, _2bx, _2bz;
        float s1, s2, s3, s4;
        float qDot1, qDot2, qDot3, qDot4;

        // Auxiliary variables to avoid repeated arithmetic
        float _2q1mx;
        float _2q1my;
        float _2q1mz;
        float _2q2mx;
        float _4bx;
        float _4bz;
        float _8bx;
        float _8bz;
        float _2q1 = 2f * q1;
        float _2q2 = 2f * q2;
        float _2q3 = 2f * q3;
        float _2q4 = 2f * q4;
        float _2q1q3 = 2f * q1 * q3;
        float _2q3q4 = 2f * q3 * q4;
        float q1q1 = q1 * q1;
        float q1q2 = q1 * q2;
        float q1q3 = q1 * q3;
        float q1q4 = q1 * q4;
        float q2q2 = q2 * q2;
        float q2q3 = q2 * q3;
        float q2q4 = q2 * q4;
        float q3q3 = q3 * q3;
        float q3q4 = q3 * q4;
        float q4q4 = q4 * q4;

        // Normalise accelerometer measurement
        norm = (float)Math.sqrt(ax * ax + ay * ay + az * az);
        if (norm == 0f) return; // handle NaN
        norm = 1 / norm;        // use reciprocal for division
        ax *= norm;
        ay *= norm;
        az *= norm;

        // Normalise magnetometer measurement
        norm = (float)Math.sqrt(mx * mx + my * my + mz * mz);
        if (norm == 0f) return; // handle NaN
        norm = 1 / norm;        // use reciprocal for division
        mx *= norm;
        my *= norm;
        mz *= norm;

        // Reference direction of Earth's magnetic field
        _2q1mx = 2f * q1 * mx;
        _2q1my = 2f * q1 * my;
        _2q1mz = 2f * q1 * mz;
        _2q2mx = 2f * q2 * mx;
        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;
        _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;
        _4bx = 2f * _2bx;
        _4bz = 2f * _2bz;
        _8bx = 2f * _4bx;
        _8bz = 2f * _4bz;

        /** Gradient decent algorithm corrective step old
        s1 = -_2q3 * (2f * q2q4 - _2q1q3 - ax) + _2q2 * (2f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
        s2 = _2q4 * (2f * q2q4 - _2q1q3 - ax) + _2q1 * (2f * q1q2 + _2q3q4 - ay) - 4f * q2 * (1 - 2f * q2q2 - 2f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
        s3 = -_2q1 * (2f * q2q4 - _2q1q3 - ax) + _2q4 * (2f * q1q2 + _2q3q4 - ay) - 4f * q3 * (1 - 2f * q2q2 - 2f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
        s4 = _2q2 * (2f * q2q4 - _2q1q3 - ax) + _2q3 * (2f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
        norm = 1f / (float)Math.sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
        */

        s1 = -_2q3 * (2f * (q2q4 -q1q3) - ax) + _2q2 * (2f *(q1q2 + q3q4) - ay) + -_4bz * q3 * (_4bx * (0.5f -q3q3 - q4q4) + _4bz * (q2q4 - q1q3) - mx) + (-_4bx * q4 + _4bz * q2) * (_4bx * (q2q3 - q1q4) + _4bz * (q1q2 + q3q4) - my) + _4bx * q3 * (_4bx * (q1q3 + q2q4) + _4bz * (0.5f - q2q2 - q3q3) - mz);
        s2 = (float)(_2q4 * (2f *(q2q4 - q1q3) - ax) + _2q1 * (2f *(q1q2 + q3q4) - ay) - 4f * q2 * (2  *(0.5 - q2q2 - q3q3) - az) + _4bz * q4 * (_4bx * (0.5f - q3q3 - q4q4) + _4bz * (q2q4 - q1q3) - mx) + (_4bx * q3 + _4bz * q1) * (_4bx * (q2q3 - q1q4) + _4bz * (q1q2 + q3q4) - my) + (_4bx * q4 - _8bz * q2) * (_4bx * (q1q3 + q2q4) + _4bz * (0.5f - q2q2 - q3q3) - mz));
        s3 = (float)(-_2q1 * (2f * (q2q4 -q1q3) - ax) + _2q4 * (2f * (q1q2 + q3q4) - ay) +( -4f *q3) * (2 * (0.5 - q2q2 - q3q3) - az) + (-_8bx * q3 - _4bz * q1) * (_4bx * (0.5f - q3q3 - q4q4) + _4bz * (q2q4 - q1q3) - mx) + (_4bx * q2 + _4bz * q4) * (_4bx * (q2q3 - q1q4) + _4bz * (q1q2 + q3q4) - my) + (_4bx * q1 - _8bz * q3) * (_4bx * (q1q3 + q2q4) + _4bz * (0.5f - q2q2 - q3q3) - mz));
        s4 = _2q2 * (2f *(q2q4 - q1q3) - ax) + _2q3 * (2f *(q1q2 + q3q4) - ay) + (-_8bx * q4 + _4bz * q2) * (_4bx * (0.5f - q3q3- q4q4) + _4bz * (q2q4 - q1q3) - mx) + (-_4bx * q1 + _4bz * q3) * (_4bx * (q2q3 - q1q4) + _4bz * (q1q2 + q3q4) - my) + (_4bx * q2) * (_4bx * (q1q3 + q2q4) + _4bz * (0.5f - q2q2 - q3q3) - mz);
        norm = 1f / (float)Math.sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);

        s1 *= norm;
        s2 *= norm;
        s3 *= norm;
        s4 *= norm;

        // Compute rate of change of quaternion
        qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - Beta * s1;
        qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - Beta * s2;
        qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - Beta * s3;
        qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - Beta * s4;

        // Integrate to yield quaternion
        q1 += qDot1 * SamplePeriod;
        q2 += qDot2 * SamplePeriod;
        q3 += qDot3 * SamplePeriod;
        q4 += qDot4 * SamplePeriod;
        norm = 1f / (float)Math.sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
        Quaternion[0] = q1 * norm;
        Quaternion[1] = q2 * norm;
        Quaternion[2] = q3 * norm;
        Quaternion[3] = q4 * norm;

    }

    public double[] computeAngles()
    {
        double roll = Math.atan2(Quaternion[0] * Quaternion[1] + Quaternion[2] * Quaternion[3], 0.5f - Quaternion[1] * Quaternion[1] - Quaternion[2] * Quaternion[2]);
        double pitch = Math.asin(-2.0f * (Quaternion[1] * Quaternion[3] - Quaternion[0] * Quaternion[2]));
        double yaw = Math.atan2(Quaternion[1] * Quaternion[2] + Quaternion[0] * Quaternion[3], 0.5f - Quaternion[2] * Quaternion[2] - Quaternion[3] * Quaternion[3]);
        return new double[] { roll,pitch,yaw };

    }
}

Charts of Input data: Input data

did you tried the original sources to compare ? http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ — minorlogic, Mar 19 '18 at 08:24
Yes as I said I am using the c# code from their site and the results are the same — Jopo Podpleský, Mar 19 '18 at 08:44

muminers · Answer 1 · 2018-03-18T12:54:30.310

Magdwick algorithm as well as eg also very popular Kalmann algorithm are using data provided by your MEMS accelerometer, gyroscope and/or magnetometer. The problem is that all those sensors have their drift and their offset. So the problem you are descibing is nothing else but drift comming aut from integration/double integration of this noise.

Both Magdwick and Kalman filters have their tools to filtrate noises from specific sensors (eg using gravity vector for gyroscope or prediction matrix for accelerometer) but they can't do much if the sensors itself aren't providing really good data. And there's actually no need to put realy good sensors in a smartphone.

There is something you could try.

Because accelerometer is probably the biggest source of drift here, you can try adding eq Butterworth filter on your accelerometer signal before passing it to the algorithm. This will probably reduce the drift but also will provide a significant inertion of output signal. Nethertheless if you are thinking of some applications where the phone will be held bu human, than there is no need to think that your accelerometer signals could grow by 10g or more.

I've developed an app calculating it's rotation. I faced the same problem and implementing Butterworth filter helped... But the output data differed a lot on different devices. It's just sensors quality that makes it so unpredictible.

thank you for response I will try it. I am well aware of the drift but what confuses me is that it works when I am moving, but it doesn't during static scenarios. (Sorry I am still very new to signal processing) — Jopo Podpleský, Mar 18 '18 at 12:59
The problem is that when you are moving, the usual state is that the drift is only a small fraction of the input signal. When device is stable than the noise becomes just more important. The way of cutting it off is to apply filter that will not allow the signal to grow to fast (eg treat acceleration change bigger than 10g as a noise). — muminers, Mar 18 '18 at 13:32

Madgwick AHRS algorithm Android implementation

1 Answers1