private void computeMatrixOfDerivatives() { double del, der, fp, fm, arg, sav; int i2, iy; fp = 0.; fm = 0.; for (int im = 0; im < m; im++) { i2 = -1; for (int il = 0; il < n + r; il++) { if (il < r) { if (list[il] != 0) { i2++; sav = x.getElement(il); arg = Math.abs(sav); if (arg < CUT) arg = CUT; del = DELTA * arg; x.setElement(il, sav + del); fp = uf.getValue(y, x, im); x.setElement(il, sav - del); fm = uf.getValue(y, x, im); der = (fp - fm) / (del + del); e.setElement(im, i2, der); x.setElement(il, sav); } } else { i2++; iy = il - r; sav = y.getElement(iy); arg = Math.abs(sav); if (arg < CUT) arg = CUT; del = DELTA * arg; y.setElement(iy, sav + del); fp = uf.getValue(y, x, im); y.setElement(iy, sav - del); fm = uf.getValue(y, x, im); der = (fp - fm) / (del + del); e.setElement(im, i2, der); y.setElement(iy, sav); } } } }
/** * @param y vector of measurements. * @param cy covaariance matrix of measurements. * @param x vector of first approximations of unknowns. * @param list array containing the elements of a list specifying which of the n variables are * fixed (list element = 0) and which are adjustable (list element = 1). m number of constaint * equations * @param uf user function which must be an extension of the abstract class DatanUserFunction. */ public LsqGen( DatanVector y, DatanMatrix cy, DatanVector x, int[] list, int m, DatanUserFunction uf) { this.y = y; this.cy = cy; this.x = x; this.list = list; this.m = m; this.uf = uf; n = y.getNumberOfElements(); r = list.length; nred = 0; for (int i = 0; i < r; i++) { if (list[i] == 1) nred++; } l = n + nred; t = new DatanVector(l); f = new DatanMatrix(cy); f.choleskyInversion(); fy = f.choleskyDecomposition(); mfvec = new DatanVector(1); ok = new boolean[1]; mf = 0.; nstep = 100; d = new DatanVector(m); loop: for (int istep = 0; istep < nstep; istep++) { mflast = mf; f = new DatanMatrix(l, l); f.putSubmatrix(nred, nred, fy); e = new DatanMatrix(m, l); computeMatrixOfDerivatives(); for (int k = 0; k < m; k++) { d.setElement(k, -uf.getValue(y, x, k)); } b = f.choleskyMultiply(t); b = b.multiply(-1.); u = f.leastSquaresWithConstraints(b, e, d, mfvec, 0., ok); converged = ok[0]; if (!converged) break loop; mf = mfvec.getElement(0); if (nred > 0) { // update x int ired = -1; for (int i = 0; i < r; i++) { if (list[i] != 0) { ired++; x.setElement(i, x.getElement(i) + u.getElement(ired)); } } } // update y and t for (int i = 0; i < n; i++) { y.setElement(i, y.getElement(i) + u.getElement(i + nred)); t.setElement(i + nred, t.getElement(i + nred) + u.getElement(i + nred)); } // test for convergence if (istep > 0 && Math.abs(mf - mflast) < mf * EPSILON + T) break loop; } if (converged) { computeMatrixOfDerivatives(); a = e.getSubmatrix(m, n, 0, nred); help = cy.multiplyWithTransposed(a); gb = a.multiply(help); gb.choleskyInversion(); if (nred > 0) { help2 = e.getSubmatrix(m, nred, 0, 0); help3 = help2.multiplyTransposedWith(gb); cx = help3.multiply(help2); cx.choleskyInversion(); // cx contains covariance matrx of unknowns } else { cx = new DatanMatrix(1, 1); // for nred ==0 the matrix cx is set to be the (1 x 1) zero matrix } a = e.getSubmatrix(m, n, 0, nred); help = a.multiply(cy); help2 = gb.multiply(a.multiply(cy)); help3 = help.multiplyTransposedWith(help2); cyimproved = cy.sub(help3); // cyimproved contains covariance matrix of "improved" measurements } }