/**
* Computes a linear interpolating function for the data set.
*
* @param x the arguments for the interpolation points
* @param y the values for the interpolation points
* @return a function which interpolates the data set
* @throws DimensionMismatchException if {@code x} and {@code y} have different sizes.
* @throws NonMonotonicSequenceException if {@code x} is not sorted in strict increasing order.
* @throws NumberIsTooSmallException if the size of {@code x} is smaller than 2.
*/
public PolynomialSplineFunction interpolate(double x[], double y[])
throws DimensionMismatchException, NumberIsTooSmallException, NonMonotonicSequenceException {
if (x.length != y.length) {
throw new DimensionMismatchException(x.length, y.length);
}
if (x.length < 2) {
throw new NumberIsTooSmallException(LocalizedFormats.NUMBER_OF_POINTS, x.length, 2, true);
}
// Number of intervals. The number of data points is n + 1.
int n = x.length - 1;
MathArrays.checkOrder(x);
// Slope of the lines between the datapoints.
final double m[] = new double[n];
for (int i = 0; i < n; i++) {
m[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]);
}
final PolynomialFunction polynomials[] = new PolynomialFunction[n];
final double coefficients[] = new double[2];
for (int i = 0; i < n; i++) {
coefficients[0] = y[i];
coefficients[1] = m[i];
polynomials[i] = new PolynomialFunction(coefficients);
}
return new PolynomialSplineFunction(x, polynomials);
}
/**
* Check that the interpolation arrays are valid. The arrays features checked by this method are
* that both arrays have the same length and this length is at least 2.
*
* @param x Interpolating points array.
* @param y Interpolating values array.
* @param abort Whether to throw an exception if {@code x} is not sorted.
* @throws DimensionMismatchException if the array lengths are different.
* @throws NumberIsTooSmallException if the number of points is less than 2.
* @throws org.apache.commons.math3.exception.NonMonotonicSequenceException if {@code x} is not
* sorted in strictly increasing order and {@code abort} is {@code true}.
* @return {@code false} if the {@code x} is not sorted in increasing order, {@code true}
* otherwise.
* @see #evaluate(double[], double[], double)
* @see #computeCoefficients()
*/
public static boolean verifyInterpolationArray(double x[], double y[], boolean abort)
throws DimensionMismatchException, NumberIsTooSmallException, NonMonotonicSequenceException {
if (x.length != y.length) {
throw new DimensionMismatchException(x.length, y.length);
}
if (x.length < 2) {
throw new NumberIsTooSmallException(
LocalizedFormats.WRONG_NUMBER_OF_POINTS, 2, x.length, true);
}
return MathArrays.checkOrder(x, MathArrays.OrderDirection.INCREASING, true, abort);
}
/**
* @param x Sample values of the x-coordinate, in increasing order.
* @param y Sample values of the y-coordinate, in increasing order.
* @param f Values of the function on every grid point.
* @param dFdX Values of the partial derivative of function with respect to x on every grid point.
* @param dFdY Values of the partial derivative of function with respect to y on every grid point.
* @param d2FdXdY Values of the cross partial derivative of function on every grid point.
* @param initializeDerivatives Whether to initialize the internal data needed for calling any of
* the methods that compute the partial derivatives this function.
* @throws DimensionMismatchException if the various arrays do not contain the expected number of
* elements.
* @throws NonMonotonicSequenceException if {@code x} or {@code y} are not strictly increasing.
* @throws NoDataException if any of the arrays has zero length.
* @see #partialDerivativeX(double,double)
* @see #partialDerivativeY(double,double)
* @see #partialDerivativeXX(double,double)
* @see #partialDerivativeYY(double,double)
* @see #partialDerivativeXY(double,double)
*/
public BicubicSplineInterpolatingFunction(
double[] x,
double[] y,
double[][] f,
double[][] dFdX,
double[][] dFdY,
double[][] d2FdXdY,
boolean initializeDerivatives)
throws DimensionMismatchException, NoDataException, NonMonotonicSequenceException {
final int xLen = x.length;
final int yLen = y.length;
if (xLen == 0 || yLen == 0 || f.length == 0 || f[0].length == 0) {
throw new NoDataException();
}
if (xLen != f.length) {
throw new DimensionMismatchException(xLen, f.length);
}
if (xLen != dFdX.length) {
throw new DimensionMismatchException(xLen, dFdX.length);
}
if (xLen != dFdY.length) {
throw new DimensionMismatchException(xLen, dFdY.length);
}
if (xLen != d2FdXdY.length) {
throw new DimensionMismatchException(xLen, d2FdXdY.length);
}
MathArrays.checkOrder(x);
MathArrays.checkOrder(y);
xval = x.clone();
yval = y.clone();
final int lastI = xLen - 1;
final int lastJ = yLen - 1;
splines = new BicubicSplineFunction[lastI][lastJ];
for (int i = 0; i < lastI; i++) {
if (f[i].length != yLen) {
throw new DimensionMismatchException(f[i].length, yLen);
}
if (dFdX[i].length != yLen) {
throw new DimensionMismatchException(dFdX[i].length, yLen);
}
if (dFdY[i].length != yLen) {
throw new DimensionMismatchException(dFdY[i].length, yLen);
}
if (d2FdXdY[i].length != yLen) {
throw new DimensionMismatchException(d2FdXdY[i].length, yLen);
}
final int ip1 = i + 1;
for (int j = 0; j < lastJ; j++) {
final int jp1 = j + 1;
final double[] beta =
new double[] {
f[i][j], f[ip1][j], f[i][jp1], f[ip1][jp1],
dFdX[i][j], dFdX[ip1][j], dFdX[i][jp1], dFdX[ip1][jp1],
dFdY[i][j], dFdY[ip1][j], dFdY[i][jp1], dFdY[ip1][jp1],
d2FdXdY[i][j], d2FdXdY[ip1][j], d2FdXdY[i][jp1], d2FdXdY[ip1][jp1]
};
splines[i][j] =
new BicubicSplineFunction(computeSplineCoefficients(beta), initializeDerivatives);
}
}
if (initializeDerivatives) {
// Compute all partial derivatives.
partialDerivatives = new BivariateFunction[5][lastI][lastJ];
for (int i = 0; i < lastI; i++) {
for (int j = 0; j < lastJ; j++) {
final BicubicSplineFunction bcs = splines[i][j];
partialDerivatives[0][i][j] = bcs.partialDerivativeX();
partialDerivatives[1][i][j] = bcs.partialDerivativeY();
partialDerivatives[2][i][j] = bcs.partialDerivativeXX();
partialDerivatives[3][i][j] = bcs.partialDerivativeYY();
partialDerivatives[4][i][j] = bcs.partialDerivativeXY();
}
}
} else {
// Partial derivative methods cannot be used.
partialDerivatives = null;
}
}
/**
* Computes an interpolating function for the data set.
*
* @param x the arguments for the interpolation points
* @param y the values for the interpolation points
* @return a function which interpolates the data set
* @throws DimensionMismatchException if {@code x} and {@code y} have different sizes.
* @throws NonMonotonicSequenceException if {@code x} is not sorted in strict increasing order.
* @throws NumberIsTooSmallException if the size of {@code x} is smaller than 3.
*/
public PolynomialSplineFunction interpolate(double x[], double y[])
throws DimensionMismatchException, NumberIsTooSmallException, NonMonotonicSequenceException {
if (x.length != y.length) {
throw new DimensionMismatchException(x.length, y.length);
}
if (x.length < 3) {
throw new NumberIsTooSmallException(LocalizedFormats.NUMBER_OF_POINTS, x.length, 3, true);
}
// Number of intervals. The number of data points is n + 1.
final int n = x.length - 1;
MathArrays.checkOrder(x);
// Differences between knot points
final double h[] = new double[n];
for (int i = 0; i < n; i++) {
h[i] = x[i + 1] - x[i];
}
final double mu[] = new double[n];
final double z[] = new double[n + 1];
mu[0] = 0d;
z[0] = 0d;
double g = 0;
for (int i = 1; i < n; i++) {
g = 2d * (x[i + 1] - x[i - 1]) - h[i - 1] * mu[i - 1];
mu[i] = h[i] / g;
z[i] =
(3d
* (y[i + 1] * h[i - 1] - y[i] * (x[i + 1] - x[i - 1]) + y[i - 1] * h[i])
/ (h[i - 1] * h[i])
- h[i - 1] * z[i - 1])
/ g;
}
// cubic spline coefficients -- b is linear, c quadratic, d is cubic (original y's are
// constants)
final double b[] = new double[n];
final double c[] = new double[n + 1];
final double d[] = new double[n];
z[n] = 0d;
c[n] = 0d;
for (int j = n - 1; j >= 0; j--) {
c[j] = z[j] - mu[j] * c[j + 1];
b[j] = (y[j + 1] - y[j]) / h[j] - h[j] * (c[j + 1] + 2d * c[j]) / 3d;
d[j] = (c[j + 1] - c[j]) / (3d * h[j]);
}
final PolynomialFunction polynomials[] = new PolynomialFunction[n];
final double coefficients[] = new double[4];
for (int i = 0; i < n; i++) {
coefficients[0] = y[i];
coefficients[1] = b[i];
coefficients[2] = c[i];
coefficients[3] = d[i];
polynomials[i] = new PolynomialFunction(coefficients);
}
return new PolynomialSplineFunction(x, polynomials);
}
/**
* Compute a weighted loess fit on the data at the original abscissae.
*
* @param xval Arguments for the interpolation points.
* @param yval Values for the interpolation points.
* @param weights point weights: coefficients by which the robustness weight of a point is
* multiplied.
* @return the values of the loess fit at corresponding original abscissae.
* @throws NonMonotonicSequenceException if {@code xval} not sorted in strictly increasing order.
* @throws DimensionMismatchException if {@code xval} and {@code yval} have different sizes.
* @throws NoDataException if {@code xval} or {@code yval} has zero size.
* @throws NotFiniteNumberException if any of the arguments and values are not finite real
* numbers.
* @throws NumberIsTooSmallException if the bandwidth is too small to accomodate the size of the
* input data (i.e. the bandwidth must be larger than 2/n).
* @since 2.1
*/
public final double[] smooth(final double[] xval, final double[] yval, final double[] weights)
throws NonMonotonicSequenceException, DimensionMismatchException, NoDataException,
NotFiniteNumberException, NumberIsTooSmallException {
if (xval.length != yval.length) {
throw new DimensionMismatchException(xval.length, yval.length);
}
final int n = xval.length;
if (n == 0) {
throw new NoDataException();
}
checkAllFiniteReal(xval);
checkAllFiniteReal(yval);
checkAllFiniteReal(weights);
MathArrays.checkOrder(xval);
if (n == 1) {
return new double[] {yval[0]};
}
if (n == 2) {
return new double[] {yval[0], yval[1]};
}
int bandwidthInPoints = (int) (bandwidth * n);
if (bandwidthInPoints < 2) {
throw new NumberIsTooSmallException(LocalizedFormats.BANDWIDTH, bandwidthInPoints, 2, true);
}
final double[] res = new double[n];
final double[] residuals = new double[n];
final double[] sortedResiduals = new double[n];
final double[] robustnessWeights = new double[n];
// Do an initial fit and 'robustnessIters' robustness iterations.
// This is equivalent to doing 'robustnessIters+1' robustness iterations
// starting with all robustness weights set to 1.
Arrays.fill(robustnessWeights, 1);
for (int iter = 0; iter <= robustnessIters; ++iter) {
final int[] bandwidthInterval = {0, bandwidthInPoints - 1};
// At each x, compute a local weighted linear regression
for (int i = 0; i < n; ++i) {
final double x = xval[i];
// Find out the interval of source points on which
// a regression is to be made.
if (i > 0) {
updateBandwidthInterval(xval, weights, i, bandwidthInterval);
}
final int ileft = bandwidthInterval[0];
final int iright = bandwidthInterval[1];
// Compute the point of the bandwidth interval that is
// farthest from x
final int edge;
if (xval[i] - xval[ileft] > xval[iright] - xval[i]) {
edge = ileft;
} else {
edge = iright;
}
// Compute a least-squares linear fit weighted by
// the product of robustness weights and the tricube
// weight function.
// See http://en.wikipedia.org/wiki/Linear_regression
// (section "Univariate linear case")
// and http://en.wikipedia.org/wiki/Weighted_least_squares
// (section "Weighted least squares")
double sumWeights = 0;
double sumX = 0;
double sumXSquared = 0;
double sumY = 0;
double sumXY = 0;
double denom = FastMath.abs(1.0 / (xval[edge] - x));
for (int k = ileft; k <= iright; ++k) {
final double xk = xval[k];
final double yk = yval[k];
final double dist = (k < i) ? x - xk : xk - x;
final double w = tricube(dist * denom) * robustnessWeights[k] * weights[k];
final double xkw = xk * w;
sumWeights += w;
sumX += xkw;
sumXSquared += xk * xkw;
sumY += yk * w;
sumXY += yk * xkw;
}
final double meanX = sumX / sumWeights;
final double meanY = sumY / sumWeights;
final double meanXY = sumXY / sumWeights;
final double meanXSquared = sumXSquared / sumWeights;
final double beta;
if (FastMath.sqrt(FastMath.abs(meanXSquared - meanX * meanX)) < accuracy) {
beta = 0;
} else {
beta = (meanXY - meanX * meanY) / (meanXSquared - meanX * meanX);
}
final double alpha = meanY - beta * meanX;
res[i] = beta * x + alpha;
residuals[i] = FastMath.abs(yval[i] - res[i]);
}
// No need to recompute the robustness weights at the last
// iteration, they won't be needed anymore
if (iter == robustnessIters) {
break;
}
// Recompute the robustness weights.
// Find the median residual.
// An arraycopy and a sort are completely tractable here,
// because the preceding loop is a lot more expensive
System.arraycopy(residuals, 0, sortedResiduals, 0, n);
Arrays.sort(sortedResiduals);
final double medianResidual = sortedResiduals[n / 2];
if (FastMath.abs(medianResidual) < accuracy) {
break;
}
for (int i = 0; i < n; ++i) {
final double arg = residuals[i] / (6 * medianResidual);
if (arg >= 1) {
robustnessWeights[i] = 0;
} else {
final double w = 1 - arg * arg;
robustnessWeights[i] = w * w;
}
}
}
return res;
}
private void appendValues(float[] incomingX, float[] incomingY, float[] incomingZ, double[] ts) {
if (this._currentIndex + ts.length > BUFFER_SIZE) {
int shift = this._currentIndex + ts.length - BUFFER_SIZE;
for (int i = 0; i < BUFFER_SIZE - shift; i++) {
_xValues[i] = _xValues[i + shift];
_yValues[i] = _yValues[i + shift];
_zValues[i] = _zValues[i + shift];
_timestamps[i] = _timestamps[i + shift];
}
int offset = BUFFER_SIZE - shift;
for (int i = 0; i < ts.length; i++) {
_xValues[offset + i] = (double) incomingX[i];
_yValues[offset + i] = (double) incomingY[i];
_zValues[offset + i] = (double) incomingZ[i];
_timestamps[offset + i] = ts[i] / 1000;
}
this._currentIndex = offset + ts.length; // Should be BUFFER_SIZE
} else {
for (int i = 0; i < ts.length; i++) {
_xValues[this._currentIndex + i] = (double) incomingX[i];
_yValues[this._currentIndex + i] = (double) incomingY[i];
_zValues[this._currentIndex + i] = (double) incomingZ[i];
_timestamps[this._currentIndex + i] = ts[i] / 1000;
}
this._currentIndex += ts.length;
}
// Log.e("PR", "Raw Timestamp: " + ts[ts.length-1]);
ArrayList<Reading> readings = new ArrayList<>();
for (int i = 0; i < this._currentIndex; i++) {
readings.add(new Reading(_timestamps[i], _xValues[i], _yValues[i], _zValues[i]));
}
boolean ordered = false;
while (ordered == false) {
try {
Collections.sort(
readings,
new Comparator<Reading>() {
public int compare(Reading one, Reading two) {
if ((two.t - one.t) > 0) return -1;
else if ((two.t - one.t) < 0) return 1;
return 0;
}
});
double[] testTimestamps = new double[readings.size()];
for (int i = 0; i < readings.size(); i++) {
Reading r = readings.get(i);
testTimestamps[i] = r.t;
if (i > 1 && testTimestamps[i] == testTimestamps[i - 1]) {
double newTime = (testTimestamps[i - 2] + testTimestamps[i]) / 2;
testTimestamps[i - 1] = newTime;
readings.get(i - 1).t = newTime;
}
}
MathArrays.checkOrder(testTimestamps);
ordered = true;
for (int i = 0; i < readings.size(); i++) {
Reading r = readings.get(i);
_timestamps[i] = r.t;
_xValues[i] = r.x;
_yValues[i] = r.y;
_zValues[i] = r.z;
}
} catch (NonMonotonicSequenceException e) {
e.printStackTrace();
}
}
}