void ProcessReading(AccelerometerReading accelerometerReading)
{
ThreeDimensionalVector lowPassFilteredAcceleration;
ThreeDimensionalVector optimalFilteredAcceleration;
ThreeDimensionalVector averagedAcceleration;
Vector3 acceleration = accelerometerReading.Acceleration;
ThreeDimensionalVector rawAcceleration = new ThreeDimensionalVector(
acceleration.X, acceleration.Y, acceleration.Z);
lock (sampleBuffer)
{
if (!initialized)
{
/* Initialize buffer with first value. */
sampleSumVector = rawAcceleration * SamplesCount;
averageAcceleration = rawAcceleration;
for (int i = 0; i < SamplesCount; i++)
{
sampleBuffer[i] = averageAcceleration;
}
previousLowPassOutput = averageAcceleration;
previousOptimalFilterOutput = averageAcceleration;
initialized = true;
}
lowPassFilteredAcceleration = new ThreeDimensionalVector(
ApplyLowPassFilter(rawAcceleration.X, previousLowPassOutput.X),
ApplyLowPassFilter(rawAcceleration.Y, previousLowPassOutput.Y),
ApplyLowPassFilter(rawAcceleration.Z, previousLowPassOutput.Z));
previousLowPassOutput = lowPassFilteredAcceleration;
optimalFilteredAcceleration = new ThreeDimensionalVector(
ApplyLowPassFilterWithNoiseThreshold(rawAcceleration.X, previousOptimalFilterOutput.X),
ApplyLowPassFilterWithNoiseThreshold(rawAcceleration.Y, previousOptimalFilterOutput.Y),
ApplyLowPassFilterWithNoiseThreshold(rawAcceleration.Z, previousOptimalFilterOutput.Z));
previousOptimalFilterOutput = optimalFilteredAcceleration;
/* Increment circular buffer insertion index. */
if (++sampleIndex >= SamplesCount)
{
/* If at max length then wrap samples back
* to the beginning position in the list. */
sampleIndex = 0;
}
/* Add new and remove old at sampleIndex. */
ThreeDimensionalVector vector = optimalFilteredAcceleration;
sampleSumVector += vector;
sampleSumVector -= sampleBuffer[sampleIndex];
sampleBuffer[sampleIndex] = vector;
averagedAcceleration = sampleSumVector / SamplesCount;
averageAcceleration = averagedAcceleration;
/* Stablity check
* If current low-pass filtered sample is deviating
* for more than 1/100 g from average
* (max of 0.5 deg inclination noise if device steady),
* then reset the stability counter.
* The calibration will be prevented until the counter is reaching
* the sample count size.
* Calibration enabled only if entire sampling buffer is "stable" */
ThreeDimensionalVector deltaAcceleration = averagedAcceleration - optimalFilteredAcceleration;
if ((Math.Abs(deltaAcceleration.X) > maximumStabilityDeltaOffset)
|| (Math.Abs(deltaAcceleration.Y) > maximumStabilityDeltaOffset)
|| (Math.Abs(deltaAcceleration.Z) > maximumStabilityDeltaOffset))
{
/* Unstable */
deviceStableCount = 0;
}
else
{
if (deviceStableCount < SamplesCount)
{
++deviceStableCount;
}
}
/* Adjust with calibration value. */
rawAcceleration += CalibrationOffset;
lowPassFilteredAcceleration += CalibrationOffset;
optimalFilteredAcceleration += CalibrationOffset;
averagedAcceleration += CalibrationOffset;
}
Reading = new EnhancedAccelerometerReading(accelerometerReading.Timestamp,
rawAcceleration,
optimalFilteredAcceleration,
lowPassFilteredAcceleration,
averagedAcceleration);
DetectShake(acceleration);
previousAcceleration = acceleration;
}
void ProcessReading(AccelerometerReading accelerometerReading)
{
ThreeDimensionalVector lowPassFilteredAcceleration;
ThreeDimensionalVector optimalFilteredAcceleration;
ThreeDimensionalVector averagedAcceleration;
Vector3 acceleration = accelerometerReading.Acceleration;
ThreeDimensionalVector rawAcceleration = new ThreeDimensionalVector(
acceleration.X, acceleration.Y, acceleration.Z);
lock (sampleBuffer)
{
if (!initialized)
{
/* Initialize buffer with first value. */
sampleSumVector = rawAcceleration * SamplesCount;
averageAcceleration = rawAcceleration;
for (int i = 0; i < SamplesCount; i++)
{
sampleBuffer[i] = averageAcceleration;
}
previousLowPassOutput = averageAcceleration;
previousOptimalFilterOutput = averageAcceleration;
initialized = true;
}
lowPassFilteredAcceleration = new ThreeDimensionalVector(
ApplyLowPassFilter(rawAcceleration.X, previousLowPassOutput.X),
ApplyLowPassFilter(rawAcceleration.Y, previousLowPassOutput.Y),
ApplyLowPassFilter(rawAcceleration.Z, previousLowPassOutput.Z));
previousLowPassOutput = lowPassFilteredAcceleration;
optimalFilteredAcceleration = new ThreeDimensionalVector(
ApplyLowPassFilterWithNoiseThreshold(rawAcceleration.X, previousOptimalFilterOutput.X),
ApplyLowPassFilterWithNoiseThreshold(rawAcceleration.Y, previousOptimalFilterOutput.Y),
ApplyLowPassFilterWithNoiseThreshold(rawAcceleration.Z, previousOptimalFilterOutput.Z));
previousOptimalFilterOutput = optimalFilteredAcceleration;
/* Increment circular buffer insertion index. */
if (++sampleIndex >= SamplesCount)
{
/* If at max length then wrap samples back
* to the beginning position in the list. */
sampleIndex = 0;
}
/* Add new and remove old at sampleIndex. */
ThreeDimensionalVector vector = optimalFilteredAcceleration;
sampleSumVector += vector;
sampleSumVector -= sampleBuffer[sampleIndex];
sampleBuffer[sampleIndex] = vector;
averagedAcceleration = sampleSumVector / SamplesCount;
averageAcceleration = averagedAcceleration;
/* Stablity check
* If current low-pass filtered sample is deviating
* for more than 1/100 g from average
* (max of 0.5 deg inclination noise if device steady),
* then reset the stability counter.
* The calibration will be prevented until the counter is reaching
* the sample count size.
* Calibration enabled only if entire sampling buffer is "stable" */
ThreeDimensionalVector deltaAcceleration = averagedAcceleration - optimalFilteredAcceleration;
if ((Math.Abs(deltaAcceleration.X) > maximumStabilityDeltaOffset) ||
(Math.Abs(deltaAcceleration.Y) > maximumStabilityDeltaOffset) ||
(Math.Abs(deltaAcceleration.Z) > maximumStabilityDeltaOffset))
{
/* Unstable */
deviceStableCount = 0;
}
else
{
if (deviceStableCount < SamplesCount)
{
++deviceStableCount;
}
}
/* Adjust with calibration value. */
rawAcceleration += CalibrationOffset;
lowPassFilteredAcceleration += CalibrationOffset;
optimalFilteredAcceleration += CalibrationOffset;
averagedAcceleration += CalibrationOffset;
}
Reading = new EnhancedAccelerometerReading(accelerometerReading.Timestamp,
rawAcceleration,
optimalFilteredAcceleration,
lowPassFilteredAcceleration,
averagedAcceleration);
DetectShake(acceleration);
previousAcceleration = acceleration;
}
