过滤陀螺仪数据的自适应算法
Adaptive algorithm for filtering gyroscope data
是否有过滤陀螺仪噪声的自适应算法？
我的应用程序目前有一个校准陀螺仪的启动对话框，它要求用户将 phone 放在 table 上 5 秒，并记录陀螺仪数据的 min/max 值在这 5 秒内收集，然后应用程序丢弃 min/max 之间的所有值，这在技术上是一个高通滤波器。
自适应算法会随时间自动确定这些 min/max 值，无需任何对话。
有点像存储最后 100 个值，并找到这些值的 min/max，但我如何知道哪些值代表移动，哪些是零移动 + 噪音？
我研究了卡尔曼滤波器，但它适用于组合的陀螺仪 + 加速度计传感器。
我的phone中的陀螺仪不仅有噪音，而且零坐标也有偏移，所以当phone完全静止时，陀螺仪报告不断的小旋转。
如果我理解正确，一个非常简单的启发式算法（例如找到数据的平均值并定义表示真实运动的阈值）应该既可以对抗偏移零坐标，又可以提供相当准确的峰值识别。
// 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
}
这是我最终得到的代码片段 (Java, Android)。
该算法采用非常大的过滤范围初始值，并逐渐减小它们，并通过将输入数据与之前的过滤范围进行比较来过滤掉运动，如果检测到运动，则丢弃最后 10 个测量值。
当 phone 静止不动在 table 上时效果最佳，但当 phone 移动和旋转时仍然可以正常工作。
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)
{
}
}
是否有过滤陀螺仪噪声的自适应算法？
我的应用程序目前有一个校准陀螺仪的启动对话框，它要求用户将 phone 放在 table 上 5 秒，并记录陀螺仪数据的 min/max 值在这 5 秒内收集，然后应用程序丢弃 min/max 之间的所有值，这在技术上是一个高通滤波器。
自适应算法会随时间自动确定这些 min/max 值，无需任何对话。
有点像存储最后 100 个值，并找到这些值的 min/max，但我如何知道哪些值代表移动，哪些是零移动 + 噪音？
我研究了卡尔曼滤波器，但它适用于组合的陀螺仪 + 加速度计传感器。
我的phone中的陀螺仪不仅有噪音，而且零坐标也有偏移，所以当phone完全静止时，陀螺仪报告不断的小旋转。
如果我理解正确，一个非常简单的启发式算法（例如找到数据的平均值并定义表示真实运动的阈值）应该既可以对抗偏移零坐标，又可以提供相当准确的峰值识别。
// 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
}
这是我最终得到的代码片段 (Java, Android)。 该算法采用非常大的过滤范围初始值，并逐渐减小它们，并通过将输入数据与之前的过滤范围进行比较来过滤掉运动，如果检测到运动，则丢弃最后 10 个测量值。
当 phone 静止不动在 table 上时效果最佳，但当 phone 移动和旋转时仍然可以正常工作。
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)
{
}
}