Адаптивный алгоритм фильтрации данных гироскопа

Question

Есть ли адаптивный алгоритм фильтрации шума гироскопа?

В настоящее время у моего приложения есть диалоговое окно запуска для калибровки гироскопа, где он запрашивает у пользователя возможность поместить телефон в таблицу на 5 секунд и записывает минимальные/максимальные значения данных гироскопа, собранные за эти 5 секунд, тогда приложение отбрасывает все значения между этим min/max, это технически фильтр верхних частот.

Адаптивный алгоритм автоматически определит эти значения min/max со временем без каких-либо диалогов.

Что-то вроде хранения последних 100 значений и нахождения min/max этих значений, но как узнать, какие значения представляют движение, а какие - нулевое движение + шум?

Я смотрел фильтр Калмана, но он предназначен для комбинированного гироскопа + датчиков акселерометра.

Гироскоп в моем телефоне не только шумный, но и сдвинутый нулевой координат, поэтому, когда телефон лежит неподвижно, гироскоп сообщает о постоянном малом вращении.

Answer 1

Вот часть кода, в которой я оказался (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) 
    { 
    } 
} 

Answer 2

Если я правильно понимаю, очень простой эвристический таких, как найти среднее значение данных и определения порога, который означает истинное движение должно как боевое смещение нуля координат и дают довольно точное распознавание пика.

// 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 будет просто поймать самый маленький из ваших пиков, в теории)

Edit:.

Я думаю, что делать такие вещи, как в среднем ваши пороги, возможно, сделает его более устойчивым к гниению из небольших пиков.

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 
}