//@Override
public void configure(DMatrixRMaj F, DMatrixRMaj Q, DMatrixRMaj H)
{
int dimenX = F.numCols;
x = new DMatrixRMaj(dimenX, 1);
P = new DMatrixRMaj(dimenX, dimenX);
eq = new Equation.Equation();
// Provide aliases between the symbolic variables and matrices we normally interact with
// The names do not have to be the same.
eq.alias(x, "x", P, "P", Q, "Q", F, "F", H, "H");
// Dummy matrix place holder to avoid compiler errors. Will be replaced later on
eq.alias(new DMatrixRMaj(1, 1), "z");
eq.alias(new DMatrixRMaj(1, 1), "R");
// Pre-compile so that it doesn't have to compile it each time it's invoked. More cumbersome
// but for small matrices the overhead is significant
predictX = eq.compile("x = F*x");
predictP = eq.compile("P = F*P*F' + Q");
updateY = eq.compile("y = z - H*x");
updateK = eq.compile("K = P*H'*inv( H*P*H' + R )");
updateX = eq.compile("x = x + K*y");
updateP = eq.compile("P = P-K*(H*P)");
}
/**
* Computes the QR decomposition of the provided matrix.
*
* @param A Matrix which is to be decomposed. Not modified.
*/
public void decompose(DMatrixRMaj A)
{
Equation.Equation eq = new Equation.Equation();
this.QR = (DMatrixRMaj)A.copy();
int N = Math.Min(A.numCols, A.numRows);
gammas = new double[A.numCols];
for (int i = 0; i < N; i++)
{
// update temporary variables
eq.alias(QR.numRows - i, "Ni", QR, "QR", i, "i");
// Place the column that should be zeroed into v
eq.process("v=QR(i:,i)");
eq.process("maxV=max(abs(v))");
// Note that v is lazily created above. Need direct access to it, which is done below.
DMatrixRMaj v = eq.lookupMatrix("v");
double maxV = eq.lookupDouble("maxV");
if (maxV > 0 && v.getNumElements() > 1)
{
// normalize to reduce overflow issues
eq.process("v=v/maxV");
// compute the magnitude of the vector
double tau = NormOps_DDRM.normF(v);
if (v.get(0) < 0)
{
tau *= -1.0;
}
eq.alias(tau, "tau");
eq.process("u_0 = v(0,0)+tau");
eq.process("gamma = u_0/tau");
eq.process("v=v/u_0");
eq.process("v(0,0)=1");
eq.process("QR(i:,i:) = (eye(Ni) - gamma*v*v')*QR(i:,i:)");
eq.process("QR(i:,i) = v");
eq.process("QR(i,i) = -1*tau*maxV");
// save gamma for recomputing Q later on
gammas[i] = eq.lookupDouble("gamma");
}
}
}
public static void main(string[] args)
{
IMersenneTwister rand = new MersenneTwisterFast(234);
Equation.Equation eq = new Equation.Equation();
eq.getFunctions().add("multTransA", createMultTransA());
SimpleMatrix <DMatrixRMaj> A = new SimpleMatrix <DMatrixRMaj>(1, 1); // will be resized
SimpleMatrix <DMatrixRMaj> B = SimpleMatrix <DMatrixRMaj> .random64(3, 4, -1, 1, rand);
SimpleMatrix <DMatrixRMaj> C = SimpleMatrix <DMatrixRMaj> .random64(3, 4, -1, 1, rand);
eq.alias(A, "A", B, "B", C, "C");
eq.process("A=multTransA(B,C)");
Console.WriteLine("Found");
Console.WriteLine(A);
Console.WriteLine("Expected");
B.transpose().mult(C).print();
}
/**
* Returns the Q matrix.
*/
public DMatrixRMaj getQ()
{
Equation.Equation eq = new Equation.Equation();
DMatrixRMaj Q = CommonOps_DDRM.identity(QR.numRows);
DMatrixRMaj u = new DMatrixRMaj(QR.numRows, 1);
int N = Math.Min(QR.numCols, QR.numRows);
eq.alias(u, "u", Q, "Q", QR, "QR", QR.numRows, "r");
// compute Q by first extracting the householder vectors from the columns of QR and then applying it to Q
for (int j = N - 1; j >= 0; j--)
{
eq.alias(j, "j", gammas[j], "gamma");
eq.process("u(j:,0) = [1 ; QR((j+1):,j)]");
eq.process("Q=(eye(r)-gamma*u*u')*Q");
}
return(Q);
}
/// <summary>
/// <p>Allows you to perform an equation in-place on this matrix by specifying the right hand side.For information on how to define an equation
/// see {@link org.ejml.equation.Equation}. The variable sequence alternates between variable and it's label String.
/// This matrix is by default labeled as 'A', but is a string is the first object in 'variables' then it will take
/// on that value.The variable passed in can be any data type supported by Equation can be passed in.
/// This includes matrices and scalars.</p>
///
/// Examples:<br/>
/// <pre>
/// perform("A = A + B", matrix,"B"); // Matrix addition
/// perform("A + B", matrix,"B"); // Matrix addition with implicit 'A = '
/// perform("A(5,:) = B", matrix,"B"); // Insert a row defined by B into A
/// perform("[A;A]"); // stack A twice with implicit 'A = '
/// perform("Q = B + 2","Q", matrix,"B"); // Specify the name of 'this' as Q
/// </pre>
/// </summary>
/// <param name="equation">String representing the symbol equation.</param>
/// <param name="variables">List of variable names and variables.</param>
public void equation(string equation, params object[] variables)
{
if (variables.Length >= 25)
{
throw new ArgumentException("Too many variables! At most 25");
}
Equation.Equation eq = new Equation.Equation();
string nameThis = "A";
int offset = 0;
if (variables.Length > 0 && variables[0] is string)
{
nameThis = (string)variables[0];
offset = 1;
if (variables.Length % 2 != 1)
{
throw new ArgumentException("Expected and odd length for variables");
}
}
else
{
if (variables.Length % 2 != 0)
{
throw new ArgumentException("Expected and even length for variables");
}
}
eq.alias(mat, nameThis);
for (int i = offset; i < variables.Length; i += 2)
{
if (!(variables[i + 1] is string))
{
throw new ArgumentException("String expected at variables index " + i);
}
object o = variables[i];
string name = (string)variables[i + 1];
//if (typeof(SimpleBase).IsAssignableFrom(o.GetType()))
if (o is SimpleBase <DMatrixRMaj> )
{
// we already know that we've got a matrix type of DMatrixRMaj
// so operations involving the same type of matrix should be okay (right?).
eq.alias(((SimpleMatrixD)o).getMatrix(), name);
}
else if (o is TMatrix)
{
eq.alias((TMatrix)o, name);
}
else if (o is TData)
{
eq.alias((TData)o, name);
}
else if (o is int)
{
eq.alias((int)o, name);
}
else
{
//string type = o == null ? "null" : o.GetType().Name;
string type = o?.GetType().Name ?? "null";
throw new ArgumentException("Variable type not supported by Equation! " + type);
}
}
// see if the assignment is implicit
if (!equation.Contains("="))
{
equation = nameThis + " = " + equation;
}
eq.process(equation);
}
