Adaptive algorithm for filtering gyroscope data

Question

Is there an adaptive algorithm for filtering gyroscope noise?

My app currently has a startup dialog to calibrate gyroscope, where it asks user to put the phone on the table for 5 seconds, and records min/max values of gyro data collected in these 5 seconds, then the app discards all values between that min/max, that's technically a high-pass filter.

The adaptive algorithm would determine these min/max values automatically over time, without any dialogs.

Something like storing last 100 values, and finding min/max of these values, but how do I know which values represent movement, and which are zero movement + noise?

I've looked into Kalman filter, but it's for a combined gyroscope + accelerometer sensors.

The gyroscope in my phone is not only noisy, but also has shifted zero coordinate, so when the phone is lying perfectly still, the gyroscope reports constant small rotation.

Answer 1

Here's the piece of code I ended up with (Java, Android). The algorithm takes very large initial values for filter range, and gradually decrease them, and it filters out movement by comparing input data to the previous filter range, and discarding 10 last measured values if it detects movement.

It works best when the phone is lying still on the table, but still works reasonably okay when the phone is moved and rotated.

class GyroscopeListener implements SensorEventListener
{
    // Noise filter with sane initial values, so user will be able
    // to move gyroscope during the first 10 seconds, while the noise is measured.
    // After that the values are replaced by noiseMin/noiseMax.
    final float filterMin[] = new float[] { -0.05f, -0.05f, -0.05f };
    final float filterMax[] = new float[] { 0.05f, 0.05f, 0.05f };

    // The noise levels we're measuring.
    // Large initial values, they will decrease, but never increase.
    float noiseMin[] = new float[] { -1.0f, -1.0f, -1.0f };
    float noiseMax[] = new float[] { 1.0f, 1.0f, 1.0f };

    // The gyro data buffer, from which we care calculating min/max noise values.
    // The bigger it is, the more precise the calclations, and the longer it takes to converge.
    float noiseData[][] = new float[200][noiseMin.length];
    int noiseDataIdx = 0;

    // When we detect movement, we remove last few values of the measured data.
    // The movement is detected by comparing values to noiseMin/noiseMax of the previous iteration.
    int movementBackoff = 0;

    // Difference between min/max in the previous measurement iteration,
    // used to determine when we should stop measuring, when the change becomes negligilbe.
    float measuredNoiseRange[] = null;

    // How long the algorithm is running, to stop it if it does not converge.
    int measurementIteration = 0;

    public GyroscopeListener(Context context)
    {
        SensorManager manager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE);
        if ( manager == null && manager.getDefaultSensor(Sensor.TYPE_GYROSCOPE) == null )
            return;
        manager.registerListener(gyro, manager.getDefaultSensor(Sensor.TYPE_GYROSCOPE),
            SensorManager.SENSOR_DELAY_GAME);
    }

    public void onSensorChanged(final SensorEvent event)
    {
        boolean filtered = true;
        final float[] data = event.values;

        if( noiseData != null )
            collectNoiseData(data);

        for( int i = 0; i < 3; i++ )
        {
            if( data[i] < filterMin[i] )
            {
                filtered = false;
                data[i] -= filterMin[i];
            }
            else if( data[i] > filterMax[i] )
            {
                filtered = false;
                data[i] -= filterMax[i];
            }
        }

        if( filtered )
            return;

        // Use the filtered gyroscope data here
    }

    void collectNoiseData(final float[] data)
    {
        for( int i = 0; i < noiseMin.length; i++ )
        {
            if( data[i] < noiseMin[i] || data[i] > noiseMax[i] )
            {
                // Movement detected, this can converge our min/max too early, so we're discarding last few values
                if( movementBackoff < 0 )
                {
                    int discard = 10;
                    if( -movementBackoff < discard )
                        discard = -movementBackoff;
                    noiseDataIdx -= discard;
                    if( noiseDataIdx < 0 )
                        noiseDataIdx = 0;
                }
                movementBackoff = 10;
                return;
            }
            noiseData[noiseDataIdx][i] = data[i];
        }
        movementBackoff--;
        if( movementBackoff >= 0 )
            return; // Also discard several values after the movement stopped
        noiseDataIdx++;

        if( noiseDataIdx < noiseData.length )
            return;

        measurementIteration++;
        if( measurementIteration > 5 )
        {
            // We've collected enough data to use our noise min/max values as a new filter
            System.arraycopy(noiseMin, 0, filterMin, 0, filterMin.length);
            System.arraycopy(noiseMax, 0, filterMax, 0, filterMax.length);
        }
        if( measurementIteration > 15 )
        {
            // Finish measuring if the algorithm cannot converge in a long time
            noiseData = null;
            measuredNoiseRange = null;
            return;
        }

        noiseDataIdx = 0;
        boolean changed = false;
        for( int i = 0; i < noiseMin.length; i++ )
        {
            float min = 1.0f;
            float max = -1.0f;
            for( int ii = 0; ii < noiseData.length; ii++ )
            {
                if( min > noiseData[ii][i] )
                    min = noiseData[ii][i];
                if( max < noiseData[ii][i] )
                    max = noiseData[ii][i];
            }
            // Increase the range a bit, for safe conservative filtering
            float middle = (min + max) / 2.0f;
            min += (min - middle) * 0.2f;
            max += (max - middle) * 0.2f;
            // Check if range between min/max is less then the current range, as a safety measure,
            // and min/max range is not jumping outside of previously measured range
            if( max - min < noiseMax[i] - noiseMin[i] && min >= noiseMin[i] && max <= noiseMax[i] )
            {
                // Move old min/max closer to the measured min/max, but do not replace the values altogether
                noiseMin[i] = (noiseMin[i] + min * 4.0f) / 5.0f;
                noiseMax[i] = (noiseMax[i] + max * 4.0f) / 5.0f;
                changed = true;
            }
        }

        if( !changed )
            return;

        // Determine when to stop measuring - check that the previous min/max range is close enough to the current one

        float range[] = new float[noiseMin.length];
        for( int i = 0; i < noiseMin.length; i++ )
            range[i] = noiseMax[i] - noiseMin[i];

        if( measuredNoiseRange == null )
        {
            measuredNoiseRange = range;
            return; // First iteration, skip further checks
        }

        for( int i = 0; i < range.length; i++ )
        {
            if( measuredNoiseRange[i] / range[i] > 1.2f )
            {
                measuredNoiseRange = range;
                return;
            }
        }

        // We converged to the final min/max filter values, stop measuring
        System.arraycopy(noiseMin, 0, filterMin, 0, filterMin.length);
        System.arraycopy(noiseMax, 0, filterMax, 0, filterMax.length);
        noiseData = null;
        measuredNoiseRange = null;
    }

    public void onAccuracyChanged(Sensor s, int a)
    {
    }
}

Answer 2

If I understand correctly, a very simple heuristic such as finding the mean of the data and defining a threshold that signifies true movement should both combat the offset zero coordinate and give pretty accurate peak recognition.

// Initialize starting mean and threshold
mean = 0
dataCount = 0
thresholdDelta = 0.1

def findPeaks(data) {
    mean = updateMean(data)

    for point in data {
        if (point > mean + thresholdDelta) || (point < mean - thresholdDelta) {
            peaks.append(point)
        }
    }
    max = peaks.max()
    min = peaks.min()

    thresholdDelta = updateThreshold(max, min, mean)

    return {max, min}
}

def updateThreshold(max, min) {
    // 1 will make threshold equal the average peak value, 0 will make threshold equal mean
    weight = 0.5

    newThreshold = (weight * (max - min)) / 2
    return newThreshold
}

def updateMean(data) {
    newMean = (sum(data) + (dataCount * mean)) / (dataCount + data.size)
    dataCount += data.size
    return newMean
}

Here we have a threshold and mean that will update over time to become more accurate to present data.

If you have peaks that vary quite strongly (say your largest of peaks can be quadruple the smallest) you would want to set your threshold weight accordingly (for our example, 0.25 would just catch the smallest of your peaks, in theory.)

Edit:

I think doing things like averaging your thresholds would probably make it more resistant to decay from small peaks.

thresholdCount = 0

def updateThreshold(max, min) {
    // 1 will make threshold equal the average peak value, 0 will make threshold equal mean
    weight = 0.5

    newThreshold = (weight * (max - min)) / 2
    averagedThreshold = (newThreshold + (thresholdCount * thresholdDelta)) / (thresholdCount + 1)
    return averagedThreshold
}