@Test
public void testCurve() {
final double[] testYValues = new double[] {20, 40, 90};
final PolynomialSplineFunction testFunction =
new SplineInterpolator().interpolate(dummyIndex, testYValues);
final double[] coords = spyRouteDrawer.getCoords(testFunction, dummyIndex);
final double stepSize =
testFunction.getKnots()[testFunction.getKnots().length - 1] / coords.length;
assertEquals(testYValues[0] * stepSize, coords[(int) Math.round(dummyIndex[0])], 1);
assertEquals(testYValues[1] * stepSize, coords[(int) Math.round(dummyIndex[1])], 1);
assertEquals(testYValues[2] * stepSize, coords[(int) Math.round(dummyIndex[2])], 1);
// TODO change the calculation so that delta = 0;
}
public static IDataset interpolate(IDataset oldx, IDataset oldy, IDataset newx) {
// TODO more sanity checks on inputs
DoubleDataset dx = (DoubleDataset) DatasetUtils.cast(oldx, Dataset.FLOAT64);
DoubleDataset dy = (DoubleDataset) DatasetUtils.cast(oldy, Dataset.FLOAT64);
boolean sorted = true;
double maxtest = oldx.getDouble(0);
for (int i = 1; i < ((Dataset) oldx).count(); i++) {
if (maxtest > oldx.getDouble(i)) {
sorted = false;
break;
}
maxtest = dx.getDouble(i);
}
double[] sortedx = null;
double[] sortedy = null;
if (!sorted) {
IntegerDataset sIdx = getIndiciesOfSorted(dx);
sortedx = new double[dx.getData().length];
sortedy = new double[dy.getData().length];
for (int i = 0; i < sIdx.getSize(); i++) {
sortedx[i] = dx.getDouble(sIdx.get(i));
sortedy[i] = dy.getDouble(sIdx.get(i));
}
} else {
sortedx = dx.getData();
sortedy = dy.getData();
}
SplineInterpolator si = new SplineInterpolator();
PolynomialSplineFunction poly = si.interpolate(sortedx, sortedy);
IDataset newy = newx.clone();
newy.setName(oldy.getName() + "_interpolated");
for (int i = 0; i < ((Dataset) newx).count(); i++) {
newy.set(poly.value(newx.getDouble(i)), i);
}
return newy;
}
public Series fit() {
SplineInterpolator ipo = new SplineInterpolator();
double[] x = new double[this.size()];
double[] y = new double[this.size()];
for (int n = 0; n < size(); ++n) {
x[n] = (double) n;
y[n] = get(n);
}
PolynomialSplineFunction psf = ipo.interpolate(x, y);
Series ret = new Series(iId, iName);
for (int n = 0; n < size(); ++n) ret.add(psf.value(Math.max(.0, n - 0.5)));
return ret;
}
protected void processData(final Context context, Bundle dataBundle) {
if (dataBundle.containsKey(ContinuousProbe.EVENT_TIMESTAMP)
&& dataBundle.containsKey("X")
&& dataBundle.containsKey("Y")
&& dataBundle.containsKey("Z")) {
double[] incomingTimes = dataBundle.getDoubleArray(ContinuousProbe.EVENT_TIMESTAMP);
float[] incomingX = dataBundle.getFloatArray("X");
float[] incomingY = dataBundle.getFloatArray("Y");
float[] incomingZ = dataBundle.getFloatArray("Z");
this.appendValues(incomingX, incomingY, incomingZ, incomingTimes);
final long now = System.currentTimeMillis();
final String key = this.featureKey();
final SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(context);
long updateInterval =
Long.parseLong(
prefs.getString(
"config_probe_" + key + "_frequency",
XYZBasicFrequencyFeature.DEFAULT_FREQUENCY))
* 1000;
if (now - this._lastUpdate > updateInterval) // add last updated
// check for config
{
this._lastUpdate = now;
LinearInterpolator interpolator = new LinearInterpolator();
double[] xs = _xValues;
double[] ys = _yValues;
double[] zs = _zValues;
double[] ts = _timestamps;
if (this._currentIndex < BUFFER_SIZE - 1) {
xs = Arrays.copyOfRange(_xValues, 0, this._currentIndex);
ys = Arrays.copyOfRange(_yValues, 0, this._currentIndex);
zs = Arrays.copyOfRange(_zValues, 0, this._currentIndex);
ts = Arrays.copyOfRange(_timestamps, 0, this._currentIndex);
}
// Log.e("PR", "FIRST RAW TIME: " + ts[0] + " LAST RAW TIME: " +
// ts[ts.length - 1]);
// Log.e("PR", "RAW TIME[0]: " + ts[0]);
// Log.e("PR", "RAW TIME[1]: " + ts[1]);
PolynomialSplineFunction fX = interpolator.interpolate(ts, xs);
PolynomialSplineFunction fY = interpolator.interpolate(ts, ys);
PolynomialSplineFunction fZ = interpolator.interpolate(ts, zs);
// double lowFreq = 0.6;
// double highFreq = 7.0;
double durationOffset = ts[0];
double bufferDuration = ts[ts.length - 1] - durationOffset;
double interval = 1.0 / 120.0;
// Log.e("PR", "TS/0: " + ts[0] + " -- TS/-1: " + ts[ts.length -
// 1] + " -- LEN TS: " + ts.length);
// Log.e("PR", "BD: " + bufferDuration + " INT: " + interval);
int twoPow = ts.length == 0 ? 0 : (32 - Integer.numberOfLeadingZeros(ts.length - 1));
int bufferSize = (int) Math.pow(2, twoPow);
// Log.e("PR", "BUFF SIZE: " + bufferSize);
final double[] _interX = new double[bufferSize];
final double[] _interY = new double[bufferSize];
final double[] _interZ = new double[bufferSize];
Arrays.fill(_interX, 0.0);
Arrays.fill(_interY, 0.0);
Arrays.fill(_interZ, 0.0);
interTimes = new double[bufferSize];
for (int i = 0; i < bufferSize; i++) {
interTimes[i] = durationOffset + (i * interval);
// Log.e("PR", "TIME REQUEST: " + time);
// Log.e("PR", "TIME DIFFERENCE: " + (oldTime - time));
if (interTimes[i] > ts[ts.length - 1]) // If the current
// timestamp is
// greater than the
// last recorded
// timestamp, set it
// to the last
// timestamp
interTimes[i] = ts[ts.length - 1];
_interX[i] = fX.value(interTimes[i]);
_interY[i] = fY.value(interTimes[i]);
_interZ[i] = fZ.value(interTimes[i]);
}
// double timeDifference = interTimes[bufferSize - 1] -
// interTimes[0];
// Log.e("PR", "INTERP TIME: " + timeDifference +
// " BUFFER SIZE: " + bufferSize);
// Log.e("PR", "FIRST INTERP TIME: " + interTimes[0] +
// " LAST INTERP TIME: " + interTimes[interTimes.length - 1]);
// Log.e("PR", "INTERP SAMPLE: " + interX[bufferSize - 1] +
// " - " + interY[bufferSize - 1] + " - " + interZ[bufferSize -
// 1]);
final double[] _dynamicX = new double[_interX.length];
final double[] _dynamicY = new double[_interY.length];
final double[] _dynamicZ = new double[_interZ.length];
final double[] _staticX = new double[_interX.length];
final double[] _staticY = new double[_interY.length];
final double[] _staticZ = new double[_interZ.length];
for (int i = 0; i < _interX.length; i++) {
if (i < 2) {
_dynamicX[i] = 0;
_dynamicY[i] = 0;
_dynamicZ[i] = 0;
_staticX[i] = 0;
_staticY[i] = 0;
_staticZ[i] = 0;
} else {
if (i == _dynamicX.length - 1) {
_dynamicX[i] = XYZBasicFrequencyFeature.bpFilter(_interX, this._xBPHistory, i, "X");
_dynamicY[i] = XYZBasicFrequencyFeature.bpFilter(_interY, this._yBPHistory, i, "Y");
_dynamicZ[i] = XYZBasicFrequencyFeature.bpFilter(_interZ, this._zBPHistory, i, "Z");
_staticX[i] = XYZBasicFrequencyFeature.lpFilter(_interX, this._xLPHistory, i, "X");
_staticY[i] = XYZBasicFrequencyFeature.lpFilter(_interY, this._yLPHistory, i, "Y");
_staticZ[i] = XYZBasicFrequencyFeature.lpFilter(_interZ, this._zLPHistory, i, "Z");
} else {
_dynamicX[i] = XYZBasicFrequencyFeature.bpFilter(_interX, this._xBPHistory, i, null);
_dynamicY[i] = XYZBasicFrequencyFeature.bpFilter(_interY, this._yBPHistory, i, null);
_dynamicZ[i] = XYZBasicFrequencyFeature.bpFilter(_interZ, this._zBPHistory, i, null);
_staticX[i] = XYZBasicFrequencyFeature.lpFilter(_interX, this._xLPHistory, i, null);
_staticY[i] = XYZBasicFrequencyFeature.lpFilter(_interY, this._yLPHistory, i, null);
_staticZ[i] = XYZBasicFrequencyFeature.lpFilter(_interZ, this._zLPHistory, i, null);
}
this._xBPHistory[1] = this._xBPHistory[0];
this._xBPHistory[0] = _dynamicX[i];
this._yBPHistory[1] = this._yBPHistory[0];
this._yBPHistory[0] = _dynamicY[i];
this._zBPHistory[1] = this._zBPHistory[0];
this._zBPHistory[0] = _dynamicZ[i];
this._xLPHistory[1] = this._xLPHistory[0];
this._xLPHistory[0] = _staticX[i];
this._yLPHistory[1] = this._yLPHistory[0];
this._yLPHistory[0] = _staticY[i];
this._zLPHistory[1] = this._zLPHistory[0];
this._zLPHistory[0] = _staticZ[i];
}
}
// Log.e("PR", "Inter Sample: " + _interX[_interX.length - 1] +
// " - " + _interY[_interX.length - 1] + " - " +
// _interZ[_interX.length - 1]);
// Log.e("PR", "DY Sample: " + _dynamicX[_dynamicX.length - 1] +
// " - " + _dynamicY[_dynamicX.length - 1] + " - " +
// _dynamicZ[_interX.length - 1]);
// Log.e("PR", "GR Sample: " + _staticX[_staticX.length - 1] +
// " - " + _staticY[_staticY.length - 1] + " - " +
// _staticZ[_staticZ.length - 1]);
double observedFreq = _interX.length / bufferDuration; // (((double)
// this._currentIndex)
// /
// bufferDuration);
// Log.e("PR", "IL: + " + _interX.length + " / BD: " +
// bufferDuration);
// Log.e("PR", "OBS HZ: " + observedFreq);
FastFourierTransformer fft = new FastFourierTransformer(DftNormalization.STANDARD);
Complex[] xFFT = fft.transform(_dynamicX, TransformType.FORWARD);
Complex[] yFFT = fft.transform(_dynamicY, TransformType.FORWARD);
Complex[] zFFT = fft.transform(_dynamicZ, TransformType.FORWARD);
double[] frequencies =
XYZBasicFrequencyFeature.calculateFreqArray(_interX.length, observedFreq);
final double[] _xMaxFreqPowPair =
XYZBasicFrequencyFeature.findPeakFrequency(xFFT, frequencies);
final double[] _yMaxFreqPowPair =
XYZBasicFrequencyFeature.findPeakFrequency(yFFT, frequencies);
final double[] _zMaxFreqPowPair =
XYZBasicFrequencyFeature.findPeakFrequency(zFFT, frequencies);
// Log.e("PR", "FREQS & GEEKS: x:" + _xMaxFreqPowPair[0] + " - "
// + _xMaxFreqPowPair[1] + " y:" + _yMaxFreqPowPair[0] + " - " +
// _yMaxFreqPowPair[1] + " z:" + _zMaxFreqPowPair[0] + " - " +
// _zMaxFreqPowPair[1] );
final XYZBasicFrequencyFeature me = this;
Runnable r =
new Runnable() {
public void run() {
Bundle data = new Bundle();
data.putDouble("TIMESTAMP", now / 1000);
data.putString("PROBE", me.name(context));
boolean incInterpolated =
prefs.getBoolean(
"config_probe_" + key + "_interpolated_enabled",
XYZBasicFrequencyFeature.INTERPOLATED_ENABLED);
boolean incBandpass =
prefs.getBoolean(
"config_probe_" + key + "_bandpass_enabled",
XYZBasicFrequencyFeature.BANDPASS_ENABLED);
boolean incLowpass =
prefs.getBoolean(
"config_probe_" + key + "_lowpass_enabled",
XYZBasicFrequencyFeature.LOWPASS_ENABLED);
if (incInterpolated || incBandpass || incLowpass) {
Bundle sensorData = new Bundle();
synchronized (me) {
sensorData.putDoubleArray("INTERP_TIMESTAMPS", interTimes);
if (incInterpolated) {
sensorData.putDoubleArray("INTER_X", _interX);
sensorData.putDoubleArray("INTER_Y", _interY);
sensorData.putDoubleArray("INTER_Z", _interZ);
}
if (incBandpass) {
sensorData.putDoubleArray("DYNAMIC_X", _dynamicX);
sensorData.putDoubleArray("DYNAMIC_Y", _dynamicY);
sensorData.putDoubleArray("DYNAMIC_Z", _dynamicZ);
}
if (incLowpass) {
sensorData.putDoubleArray("STATIC_X", _staticX);
sensorData.putDoubleArray("STATIC_Y", _staticY);
sensorData.putDoubleArray("STATIC_Z", _staticZ);
}
data.putBundle("CALCULATIONS", sensorData);
}
}
data.putDouble("WINDOW_TIMESTAMP", interTimes[0]);
data.putDouble("POWER_X", _xMaxFreqPowPair[1]);
data.putDouble("POWER_Y", _yMaxFreqPowPair[1]);
data.putDouble("POWER_Z", _zMaxFreqPowPair[1]);
data.putDouble("FREQ_X", _xMaxFreqPowPair[0]);
data.putDouble("FREQ_Y", _yMaxFreqPowPair[0]);
data.putDouble("FREQ_Z", _zMaxFreqPowPair[0]);
me.transmitData(context, data);
}
};
Thread t = new Thread(r);
t.start();
}
}
}
