private GeneralMatrix getDxMatrix(double tolerance, int maxSteps)
throws FactoryException {
/**
* Matrix of new calculated coefficients
*/
GeneralMatrix xNew = new GeneralMatrix(6, 1);
/**
* Matrix of coefficients calculated in previous iteration
*/
GeneralMatrix xOld = new GeneralMatrix(6, 1);
/**
* Difference between each steps of iteration
*/
GeneralMatrix dxMatrix = new GeneralMatrix(6, 1);
/**
* Zero matrix
*/
GeneralMatrix zero = new GeneralMatrix(6, 1);
zero.setZero();
/**
* Result
*/
GeneralMatrix xk = new GeneralMatrix(6 + valueConstrain.size(), 1);
// i is a number of iterations
int i = 0;
// iteration
do {
xOld.set(new double[] { sx, sy, phix, phiy, tx, ty });
GeneralMatrix A = getA();
GeneralMatrix l = getL();
GeneralMatrix AT = A.clone();
AT.transpose();
GeneralMatrix ATA = new GeneralMatrix(6, 6);
GeneralMatrix ATl = new GeneralMatrix(6, 1);
ATA.mul(AT, A);
ATl.mul(AT, l);
/**constrains**/
GeneralMatrix AB = createAB(ATA, getB());
AB.invert();
AB.negate();
GeneralMatrix AU = createAU(ATl, getU());
xk.mul(AB, AU);
xk.copySubMatrix(0, 0, 6, xk.getNumCol(), 0, 0, dxMatrix);
dxMatrix.negate();