public static double[] polyFit(DoubleMatrix t, DoubleMatrix y, final int degree)
throws IllegalArgumentException {
logger.setLevel(Level.INFO);
if (t.length != y.length || !t.isVector() || !y.isVector()) {
logger.error("polyfit: require same size vectors.");
throw new IllegalArgumentException("polyfit: require same size vectors.");
}
// Normalize _posting_ for numerical reasons
final int numOfPoints = t.length;
// Check redundancy
final int numOfUnknowns = degree + 1;
logger.debug("Degree of interpolating polynomial: {}", degree);
logger.debug("Number of unknowns: {}", numOfUnknowns);
logger.debug("Number of data points: {}", numOfPoints);
if (numOfPoints < numOfUnknowns) {
logger.error("Number of points is smaller than parameters solved for.");
throw new IllegalArgumentException("Number of points is smaller than parameters solved for.");
}
// Set up system of equations to solve coeff :: Design matrix
logger.debug("Setting up linear system of equations");
DoubleMatrix A = new DoubleMatrix(numOfPoints, numOfUnknowns);
// work with columns
for (int j = 0; j <= degree; j++) {
A.putColumn(j, pow(t, j));
}
// Fit polynomial through computed vector of phases
logger.debug("Solving lin. system of equations with Cholesky.");
DoubleMatrix N = A.transpose().mmul(A);
DoubleMatrix rhs = A.transpose().mmul(y);
// solution seems to be OK up to 10^-09!
DoubleMatrix x = Solve.solveSymmetric(N, rhs);
DoubleMatrix Qx_hat = Solve.solveSymmetric(N, DoubleMatrix.eye(N.getRows()));
double maxDeviation = (N.mmul(Qx_hat).sub(DoubleMatrix.eye(Qx_hat.rows))).normmax();
logger.debug("polyfit orbit: max(abs(N*inv(N)-I)) = " + maxDeviation);
// ___ report max error... (seems sometimes this can be extremely large) ___
if (maxDeviation > 1e-6) {
logger.warn("polyfit orbit: max(abs(N*inv(N)-I)) = {}", maxDeviation);
logger.warn("polyfit orbit interpolation unstable!");
}
// work out residuals
DoubleMatrix y_hat = A.mmul(x);
DoubleMatrix e_hat = y.sub(y_hat);
// 0.05 is already 1 wavelength! (?)
if (e_hat.normmax() > 0.02) {
logger.warn(
"WARNING: Max. approximation error at datapoints (x,y,or z?): {}", e_hat.normmax());
} else {
logger.debug("Max. approximation error at datapoints (x,y,or z?): {}", e_hat.normmax());
}
if (logger.isDebugEnabled()) {
logger.debug("REPORTING POLYFIT LEAST SQUARES ERRORS");
logger.debug(" time \t\t\t y \t\t\t yhat \t\t\t ehat");
for (int i = 0; i < numOfPoints; i++) {
logger.debug(" " + t.get(i) + "\t" + y.get(i) + "\t" + y_hat.get(i) + "\t" + e_hat.get(i));
}
for (int i = 0; i < numOfPoints - 1; i++) {
// ___ check if dt is constant, not necessary for me, but may ___
// ___ signal error in header data of SLC image ___
double dt = t.get(i + 1) - t.get(i);
logger.debug("Time step between point " + i + 1 + " and " + i + "= " + dt);
if (Math.abs(dt - (t.get(1) - t.get(0))) > 0.001) // 1ms of difference we allow...
logger.warn("WARNING: Orbit: data does not have equidistant time interval?");
}
}
return x.toArray();
}
