//@Override
public void update(DMatrixRMaj z, DMatrixRMaj R)
{
// y = z - H x
CommonOps_DDRM.mult(H, x, y);
CommonOps_DDRM.subtract(z, y, y);
// S = H P H' + R
CommonOps_DDRM.mult(H, P, c);
CommonOps_DDRM.multTransB(c, H, S);
CommonOps_DDRM.addEquals(S, R);
// K = PH'S^(-1)
if (!solver.setA(S))
{
throw new InvalidOperationException("Invert failed");
}
solver.invert(S_inv);
CommonOps_DDRM.multTransA(H, S_inv, d);
CommonOps_DDRM.mult(P, d, K);
// x = x + Ky
CommonOps_DDRM.mult(K, y, a);
CommonOps_DDRM.addEquals(x, a);
// P = (I-kH)P = P - (KH)P = P-K(HP)
CommonOps_DDRM.mult(H, P, c);
CommonOps_DDRM.mult(K, c, b);
CommonOps_DDRM.subtractEquals(P, b);
}
public override void solve(DMatrixRMaj B, DMatrixRMaj X)
{
if (X.numRows != numCols)
{
throw new ArgumentException("Unexpected dimensions for X");
}
else if (B.numRows != numRows || B.numCols != X.numCols)
{
throw new ArgumentException("Unexpected dimensions for B");
}
int BnumCols = B.numCols;
// get the pivots and transpose them
int[] pivots = decomposition.getColPivots();
// solve each column one by one
for (int colB = 0; colB < BnumCols; colB++)
{
x_basic.reshape(numRows, 1);
Y.reshape(numRows, 1);
// make a copy of this column in the vector
for (int i = 0; i < numRows; i++)
{
Y.data[i] = B.get(i, colB);
}
// Solve Q*a=b => a = Q'*b
CommonOps_DDRM.multTransA(Q, Y, x_basic);
// solve for Rx = b using the standard upper triangular solver
TriangularSolver_DDRM.solveU(R11.data, x_basic.data, rank);
// finish the basic solution by filling in zeros
x_basic.reshape(numCols, 1, true);
for (int i = rank; i < numCols; i++)
{
x_basic.data[i] = 0;
}
if (norm2Solution && rank < numCols)
{
upgradeSolution(x_basic);
}
// save the results
for (int i = 0; i < numCols; i++)
{
X.set(pivots[i], colB, x_basic.data[i]);
}
}
}
/**
* Converts a vector from eigen space into sample space.
*
* @param eigenData Eigen space data.
* @return Sample space projection.
*/
public double[] eigenToSampleSpace(double[] eigenData)
{
if (eigenData.Length != numComponents)
{
throw new ArgumentException("Unexpected sample length");
}
DMatrixRMaj s = new DMatrixRMaj(A.getNumCols(), 1);
DMatrixRMaj r = DMatrixRMaj.wrap(numComponents, 1, eigenData);
CommonOps_DDRM.multTransA(V_t, r, s);
DMatrixRMaj mean = DMatrixRMaj.wrap(A.getNumCols(), 1, this.mean);
CommonOps_DDRM.add(s, mean, s);
return(s.data);
}
private void solveEigenvectorDuplicateEigenvalue(double real, int first, bool isTriangle)
{
double scale = Math.Abs(real);
if (scale == 0)
{
scale = 1;
}
eigenvectorTemp.reshape(N, 1, false);
eigenvectorTemp.zero();
if (first > 0)
{
if (isTriangle)
{
solveUsingTriangle(real, first, eigenvectorTemp);
}
else
{
solveWithLU(real, first, eigenvectorTemp);
}
}
eigenvectorTemp.reshape(N, 1, false);
for (int i = first; i < N; i++)
{
Complex_F64 c = _implicit.eigenvalues[N - i - 1];
if (c.isReal() && Math.Abs(c.real - real) / scale < 100.0 * UtilEjml.EPS)
{
eigenvectorTemp.data[i] = 1;
DMatrixRMaj v = new DMatrixRMaj(N, 1);
CommonOps_DDRM.multTransA(Q, eigenvectorTemp, v);
eigenvectors[N - i - 1] = v;
NormOps_DDRM.normalizeF(v);
eigenvectorTemp.data[i] = 0;
}
}
}
/**
* Create the function. Be sure to handle all possible input types and combinations correctly and provide
* meaningful error messages. The output matrix should be resized to fit the inputs.
*/
public static ManagerFunctions.InputN createMultTransA()
{
return(new ManagerFunctions.InputN((l, m) =>
{
var inputs = l;
var manager = m;
if (inputs.Count != 2)
{
throw new InvalidOperationException("Two inputs required");
}
Variable varA = inputs[0];
Variable varB = inputs[1];
Operation.Info ret = new Operation.Info();
if (varA is VariableMatrix && varB is VariableMatrix)
{
// The output matrix or scalar variable must be created with the provided manager
VariableMatrix output = manager.createMatrix();
ret.output = output;
ret.op = new Operation("multTransA-mm")
{
process = () =>
{
DMatrixRMaj mA = ((VariableMatrix)varA).matrix;
DMatrixRMaj mB = ((VariableMatrix)varB).matrix;
output.matrix.reshape(mA.numCols, mB.numCols);
CommonOps_DDRM.multTransA(mA, mB, output.matrix);
}
};
}
else
{
throw new ArgumentException("Expected both inputs to be a matrix");
}
return ret;
}));
}
public override /**/ double quality()
{
return(SpecializedOps_DDRM.qualityTriangular(R));
}
/**
* Solves for X using the QR decomposition.
*
* @param B A matrix that is n by m. Not modified.
* @param X An n by m matrix where the solution is written to. Modified.
*/
public override void solve(DMatrixRMaj B, DMatrixRMaj X)
{
if (X.numRows != numCols)
{
throw new ArgumentException("Unexpected dimensions for X");
}
else if (B.numRows != numRows || B.numCols != X.numCols)
{
throw new ArgumentException("Unexpected dimensions for B");
}
int BnumCols = B.numCols;
Y.reshape(numRows, 1, false);
Z.reshape(numRows, 1, false);
// solve each column one by one
for (int colB = 0; colB < BnumCols; colB++)
{
// make a copy of this column in the vector
for (int i = 0; i < numRows; i++)
{
Y.data[i] = B.get(i, colB);
}
// Solve Qa=b
// a = Q'b
CommonOps_DDRM.multTransA(Q, Y, Z);
// solve for Rx = b using the standard upper triangular solver
TriangularSolver_DDRM.solveU(R.data, Z.data, numCols);
// save the results
for (int i = 0; i < numCols; i++)
{
X.set(i, colB, Z.data[i]);
}
}
}
public void multTransA(Matrix A, Matrix B, Matrix output)
{
CommonOps_DDRM.multTransA((DMatrixRMaj)A, (DMatrixRMaj)B, (DMatrixRMaj)output);
}
