public static Coordinates <W> globalENUInstance(SimpleMatrix <W> ecef)
{
Coordinates <W> c = new Coordinates <W>();
c.enu = ecef.copy();
return(c);
}
/**
* Computes the QR decomposition of the provided matrix.
*
* @param A Matrix which is to be decomposed. Not modified.
*/
public void decompose(SimpleMatrix <DMatrixRMaj> A)
{
this.QR = A.copy() as SimpleMatrix <DMatrixRMaj>;
int N = Math.Min(A.numCols(), A.numRows());
gammas = new double[A.numCols()];
for (int i = 0; i < N; i++)
{
// use extract matrix to get the column that is to be zeroed
SimpleMatrix <DMatrixRMaj> v = QR.extractMatrix(i, SimpleMatrix <DMatrixRMaj> .END, i, i + 1) as SimpleMatrix <DMatrixRMaj>;
double max = v.elementMaxAbs();
if (max > 0 && v.getNumElements() > 1)
{
// normalize to reduce overflow issues
v = v.divide(max) as SimpleMatrix <DMatrixRMaj>;
// compute the magnitude of the vector
double tau = v.normF();
if (v.get(0) < 0)
{
tau *= -1.0;
}
double u_0 = v.get(0) + tau;
double gamma = u_0 / tau;
v = v.divide(u_0) as SimpleMatrix <DMatrixRMaj>;
v.set(0, 1.0);
// extract the submatrix of A which is being operated on
SimpleBase <DMatrixRMaj> A_small = QR.extractMatrix(i, SimpleMatrix <DMatrixRMaj> .END, i, SimpleMatrix <DMatrixRMaj> .END);
// A = (I - γ*u*u<sup>T</sup>)A
A_small = A_small.plus(-gamma, v.mult(v.transpose()).mult(A_small));
// save the results
QR.insertIntoThis(i, i, A_small);
QR.insertIntoThis(i + 1, i, v.extractMatrix(1, SimpleMatrix <DMatrixRMaj> .END, 0, 1));
// Alternatively, the two lines above can be replaced with in-place equations
// READ THE JAVADOC TO UNDERSTAND HOW THIS WORKS!
// QR.equation("QR(i:,i:) = A","QR",i,"i",A_small,"A");
// QR.equation("QR((i+1):,i) = v(1:,0)","QR",i,"i",v,"v");
// save gamma for recomputing Q later on
gammas[i] = gamma;
}
}
}