过滤陀螺仪数据的自适应算法

Question

是否有过滤陀螺仪噪声的自适应算法？

我的应用目前有一个校准陀螺仪的启动对话框，它要求用户将手机放在桌子上 5 秒，并记录这 5 秒内收集的陀螺仪数据的最小/最大值，然后应用丢弃所有值在该最小值/最大值之间，从技术上讲，这是一个高通滤波器。

自适应算法会随着时间的推移自动确定这些最小值/最大值，无需任何对话。

类似于存储最后 100 个值，并找到这些值的最小值/最大值，但我如何知道哪些值代表运动，哪些是零运动 + 噪声？

我研究了卡尔曼滤波器，但它是用于组合陀螺仪 + 加速度计传感器的。

我手机中的陀螺仪不仅噪音大，而且还偏移了零坐标，所以当手机完全静止时，陀螺仪会报告不断的小旋转。

Answer 1

如果我理解正确，一个非常简单的启发式方法，例如找到数据的平均值并定义一个表示真实运动的阈值，应该既能对抗偏移零坐标，又能提供相当准确的峰值识别。

// 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
}

这里我们有一个阈值，意思是它会随着时间的推移而更新，以更准确地呈现数据。

如果您的峰变化很大（例如，您的最大峰可能是最小峰的四倍），您需要相应地设置阈值权重（对于我们的示例，0.25 将仅捕获最小的理论上你的峰值。）

编辑：

我认为做平均阈值之类的操作可能会使其更能抵抗小峰值的衰减。

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
}

Answer 2

这是我最终得到的一段代码（Java、Android）。 该算法采用非常大的过滤范围初始值，并逐渐减小它们，并通过将输入数据与之前的过滤范围进行比较来过滤掉移动，如果检测到移动则丢弃最后10个测量值。

当手机静止在桌子上时效果最好，但在手机移动和旋转时仍然可以正常工作。

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)
    {
    }
}