Adaptacyjny algorytm filtrowania danych żyroskopowych

Question

Czy istnieje algorytm adaptacyjny do filtrowania szumów żyroskopu?

Moja aplikacja ma teraz okno dialogowe startowe do kalibracji żyroskopu, gdzie prosi użytkownika o umieszczenie telefonu na stole przez 5 sekund i zapisuje wartości min/max danych żyroskopu zebrane w tych 5 sekund, a następnie aplikacja odrzuca wszystko wartości pomiędzy tym minimum/maksimum, to technicznie filtr górnoprzepustowy.

Adaptacyjny algorytm określałby te wartości min./maks. Automatycznie w czasie, bez żadnych dialogów.

Coś jak przechowywanie ostatnich 100 wartości i znalezienie min/maks. Tych wartości, ale skąd mam wiedzieć, które wartości reprezentują ruch, a które są zerowym ruchem + szumem?

Sprawdziłem filtr Kalmana, ale jest on przeznaczony dla połączonych czujników żyroskopowych i czujników przyspieszenia.

Żyroskop w moim telefonie jest nie tylko głośny, ale również ma przesuniętą współrzędną zerową, więc gdy telefon leży idealnie nieruchomo, żyroskop podaje ciągły niewielki obrót.

Answer 1

Oto fragment kodu, który otrzymałem (Java, Android). Algorytm pobiera bardzo duże wartości początkowe dla zakresu filtrów i stopniowo je zmniejsza, i filtruje ruch przez porównywanie danych wejściowych z poprzednim zakresem filtrowania i odrzuca 10 ostatnich zmierzonych wartości, jeśli wykryje ruch.

Działa najlepiej, gdy telefon leży nadal na stole, ale nadal działa poprawnie, gdy telefon jest przenoszony i obracany.

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

Jeśli dobrze rozumiem, to bardzo proste heurystyczne, takich jak znalezienie średnią z danych i definiowanie progu oznacza, że ​​prawdziwy ruch powinien zarówno walka przesunięcie zera współrzędnych i dają bardzo dokładne rozpoznawanie szczytową.

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

Tutaj mamy próg i średnią, która będzie aktualizować się z czasem, aby dokładniej prezentować dane.

Jeśli masz pików, które różnią się dość mocno (powiedzmy Twój największy pików może być czterokrotnie najmniejszy) byś chciał ustawić swoją wagę progową odpowiednio (w naszym przykładzie, 0,25 zadaniem będzie tylko złapać najmniejszy swoich szczytów, w teorii)

EDIT:.

myślę robi takie rzeczy średnio swoje progi prawdopodobnie uczynić go bardziej odporne na próchnicę od małych pików.

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 
}