319320321322323324325326327328329
/**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);
320321322323324325326327328329330