public static void JacobiMethod(decimal[,] A, decimal accuracy)
{
int rows = A.GetLength(0);
int cols = A.GetLength(1);
decimal sigma = 0;
int i_max = 0, j_max = 1;
/*m==n and A symmetric*/
while (sigma > accuracy)
{
/*find max off-diagonal element */
for (int i = 0; i < rows; i++)
{
for (int j = i + 1; j < cols; j++)
{
if (i != j && MathDecimal.Abs(A[i_max, j_max]) < MathDecimal.Abs(A[i, j]))
{
i_max = i;
j_max = j;
}
}
}
}
}
public static decimal[,] Cholesky(decimal[,] A)
{
int rows = A.GetLength(0);
int cols = A.GetLength(1);
decimal[,] L = new decimal[rows, cols];/*lower triangular*/
/*if (rows==cols & MathDecimal.Transpose(B)=B)*/
for (int k = 0; k < cols; k++)
{
decimal sum = 0;
for (int p = 0; p <= k - 2; p++)
{
sum += L[k, p] * L[k, p];
}
L[k, k] = MathDecimal.Sqrt(A[k, k] - sum);
for (int j = k; j < cols; j++)
{
sum = 0;
for (int p = 0; p <= k - 2; p++)
{
sum += L[k, p] * L[j, p];
}
A[j, k] = (A[k, j] - sum) / L[k, k];
}
}
return(L);
}
public static decimal[] LinearLeastSquares(decimal[,] A, decimal[] r)
{
decimal[,] B = MathDecimal.Prod(MathDecimal.Transpose(A), A);
decimal[] y = MathDecimal.Prod(MathDecimal.Transpose(A), r);
decimal[] res = LinearAlgebra.Gauss(B, y);
return(res);
}
public static decimal[] GaussNewton(Func <decimal[], decimal[, ]> J, Func <decimal[], decimal[]> r, decimal[] p0)
{
decimal[] p = p0;
decimal a;
int iter = 0;
while (iter < 100) //TODO: while error is large
{
a = 1;
decimal[] pAdd = LinearLeastSquares(J(p), MathDecimal.Negative(r(p)), "SVD");
while (MathDecimal.SquaredNorm2(r(p)) - MathDecimal.SquaredNorm2(r(MathDecimal.Sum(p, MathDecimal.Prod(a, pAdd)))) <
1M / 2M * a * MathDecimal.SquaredNorm2(MathDecimal.Prod(J(p), pAdd)) && a >= 0.0000000000001M)
{
a /= 2;
}
p = MathDecimal.Sum(p, MathDecimal.Prod(a, pAdd));
iter++;
}
return(p);
}
public static decimal incl(decimal[] p)
{
decimal incl;
/*if (90 - 360 / (2 * MathDecimal.PI) *
* MathDecimal.ACos(p[2]/ MathDecimal.Norm2(p)) > 60)
* {
* incl = 90 - 360 / (2 * MathDecimal.PI) *
* MathDecimal.ASin(MathDecimal.Sqrt((p[0] * p[0] + p[1] * p[1])
* / MathDecimal.Norm2(p)));
* }
* else
* {
* incl = 90 - 360 / (2 * MathDecimal.PI) *
* MathDecimal.ACos(p[2]
* / MathDecimal.Norm2(p));
* }*/
if (p[2] == 0)
{
incl = 0;
}
else
{
incl = 90 - 360 / (2 * MathDecimal.PI) *
MathDecimal.ACos(p[2]
/ MathDecimal.Norm2(p));
}
return(incl);
}
public static decimal[] Gauss(decimal[,] A, decimal[] y)
{
decimal l, sum, temp;
decimal[] res = new decimal[y.Length];
int n = A.GetLength(0);
for (int i = 0; i < n - 1; i++)
{
int maxIndex = i;
//Find the index of the row with maximal element
for (int j = i + 1; j < n; j++)
{
if (MathDecimal.Abs(A[j, i]) > MathDecimal.Abs(A[maxIndex, i]))
{
maxIndex = j;
}
}
//Change the rows with indices i and maxIndex
for (int j = i; j < n; j++)
{
temp = A[i, j];
A[i, j] = A[maxIndex, j];
A[maxIndex, j] = temp;
}
temp = y[i];
y[i] = y[maxIndex];
y[maxIndex] = temp;
//Eliminate the elements under the main diagonal in the i-th column
for (int j = i + 1; j < n; j++)
{
l = A[j, i] / A[i, i];
A[j, i] = 0;
for (int k = i + 1; k < n; k++)
{
A[j, k] -= l * A[i, k];
}
y[j] -= l * y[i];
}
}
//Backward substitution
for (int i = n - 1; i >= 0; i--)
{
sum = 0;
for (int j = i + 1; j < n; j++)
{
sum += A[i, j] * res[j];
}
res[i] = (y[i] - sum) / A[i, i];
}
return(res);
}
public static decimal tf(decimal[] p)
{
decimal tf;
if (MathDecimal.ATan2(p[1], p[0]) > -0.1M)
{
tf = 360 / (2 * MathDecimal.PI) * MathDecimal.ATan2(p[1], p[0]);
}
else
{
tf = 360 + 360 / (2 * MathDecimal.PI) * MathDecimal.ATan2(p[1], p[0]);
}
return(tf);
}
public override decimal[] computeR(decimal[] p)
{
//TODO: Rewrite computeR!!!
decimal[,] M = { { p[0], p[1], p[2] }, { p[3], p[4], p[5] }, { p[6], p[7], p[8] } };
decimal[] b = { p[9], p[10], p[11] };
decimal[] res = new decimal[data.Count * 2];/*rewrite length ...*/
//res=this.data[0].data;
for (int i = 0; i < res.GetLength(0) / 2; i++)
{
res[2 * i] = tf(MathDecimal.Sum(MathDecimal.Prod(M, data[i].data), b)) - (decimal)data[i].tf;
res[2 * i + 1] = incl(MathDecimal.Sum(MathDecimal.Prod(M, data[i].data), b)) - (decimal)data[i].incl;
}
return(res);
}
public override decimal[] calibrate()
{
Func <decimal[], decimal[, ]> J = new Func <decimal[], decimal[, ]>(computeJ);
Func <decimal[], decimal[]> r = new Func <decimal[], decimal[]>(computeR);
decimal[] p = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0 };
decimal[,] M, Mfinal = new decimal[3, 3] {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
};
decimal[] b, bFinal = new decimal[3] {
0, 0, 0
};
for (int j = 0; j < 10; j++)
{
p = Optimization.GaussNewton(J, r, p);
M = new decimal[, ] {
{ p[0], p[1], p[2] }, { p[3], p[4], p[5] }, { p[6], p[7], p[8] }
};
b = new decimal[] { p[9], p[10], p[11] };
Mfinal = MathDecimal.Prod(Mfinal, M);
bFinal = MathDecimal.Sum(MathDecimal.Prod(M, bFinal), b);
for (int i = 0; i < data.Count; i++)
{
data[i].data = MathDecimal.Sum(b, MathDecimal.Prod(M, data[i].data));
}
//scale with respect to the gravitational acceleration
/*decimal[] rGravity = new decimal[data.Count];
* for (int i = 0; i < rGravity.GetLength(0); i++)
* rGravity[i] = Preferences.G;
* decimal[,] M = new decimal[,] { { p[0], p[1], p[2] }, { p[3], p[4], p[5] }, { p[6], p[7], p[8] } };
* decimal[] b = new decimal[] { p[9], p[10], p[11] };
* decimal[,] JGravity = new decimal[data.Count,1];
* for (int i = 0; i < rGravity.GetLength(0); i++)
* JGravity[i,1] = MathDecimal.Norm2(MathDecimal.Sum(MathDecimal.Prod(M, data[i].data),b));
* decimal Q = Optimization.LinearLeastSquares(JGravity, rGravity)[1];
* p = MathDecimal.Prod(Q, p);*/
}
pars = new CalibrationParameter(Mfinal[0, 0], Mfinal[0, 1], Mfinal[0, 2], Mfinal[1, 0], Mfinal[1, 1], Mfinal[1, 2], Mfinal[2, 0], Mfinal[2, 1], Mfinal[2, 2], bFinal[0], bFinal[1], bFinal[2]);
return(p);
}
public override decimal[,] computeJ(decimal[] p)
{
//TODO: Rewrite computeJ!!!
decimal[,] M = { { p[0], p[1], p[2] }, { p[3], p[4], p[5] }, { p[6], p[7], p[8] } };
decimal[] b = { p[9], p[10], p[11] };
decimal[,] res = new decimal[data.Count * 2, 12];/*rewrite length ...*/
decimal B1, B2, B3, B, A;
for (int i = 0; i < res.GetLength(0) / 2; i++)
{
B1 = p[0] * data[i].data[0] + p[1] * data[i].data[1] + p[2] * data[i].data[2] + p[9];
B2 = p[3] * data[i].data[0] + p[4] * data[i].data[1] + p[5] * data[i].data[2] + p[10];
B3 = p[6] * data[i].data[0] + p[7] * data[i].data[1] + p[8] * data[i].data[2] + p[11];
B = B1 * B1 + B2 * B2 + B3 * B3;
A = B1 * B1 + B2 * B2;
res[2 * i, 0] = (180 * data[i].data[0] * B2) / (MathDecimal.PI * A);
res[2 * i, 1] = (180 * data[i].data[1] * B2) / (MathDecimal.PI * A);
res[2 * i, 2] = (180 * data[i].data[2] * B2) / (MathDecimal.PI * A);
res[2 * i, 3] = -(180 * data[i].data[0] * B1) / (MathDecimal.PI * A);
res[2 * i, 4] = -(180 * data[i].data[1] * B1) / (MathDecimal.PI * A);
res[2 * i, 5] = -(180 * data[i].data[2] * B1) / (MathDecimal.PI * A);
res[2 * i, 6] = 0;
res[2 * i, 7] = 0;
res[2 * i, 8] = 0;
res[2 * i, 9] = (180 * B2) / (MathDecimal.PI * A);
res[2 * i, 10] = -(180 * B1) / (MathDecimal.PI * A);
res[2 * i, 11] = 0;
res[2 * i + 1, 0] = -(180 * data[i].data[0] * B1 * B3) / (MathDecimal.PI * B * MathDecimal.Sqrt(A));
res[2 * i + 1, 1] = -(180 * data[i].data[1] * B1 * B3) / (MathDecimal.PI * B * MathDecimal.Sqrt(A));
res[2 * i + 1, 2] = -(180 * data[i].data[2] * B1 * B3) / (MathDecimal.PI * B * MathDecimal.Sqrt(A));
res[2 * i + 1, 3] = -(180 * data[i].data[0] * B2 * B3) / (MathDecimal.PI * B * MathDecimal.Sqrt(A));
res[2 * i + 1, 4] = -(180 * data[i].data[1] * B2 * B3) / (MathDecimal.PI * B * MathDecimal.Sqrt(A));
res[2 * i + 1, 5] = -(180 * data[i].data[2] * B2 * B3) / (MathDecimal.PI * B * MathDecimal.Sqrt(A));
res[2 * i + 1, 6] = (180 * data[i].data[0] * MathDecimal.Sqrt(A)) / (MathDecimal.PI * B);
res[2 * i + 1, 7] = (180 * data[i].data[1] * MathDecimal.Sqrt(A)) / (MathDecimal.PI * B);
res[2 * i + 1, 8] = (180 * data[i].data[2] * MathDecimal.Sqrt(A)) / (MathDecimal.PI * B);
res[2 * i + 1, 9] = -(180 * B1 * B3) / (MathDecimal.PI * B * MathDecimal.Sqrt(A));
res[2 * i + 1, 10] = -(180 * B2 * B3) / (MathDecimal.PI * B * MathDecimal.Sqrt(A));;
res[2 * i + 1, 11] = (180 * MathDecimal.Sqrt(A)) / (MathDecimal.PI * B);
}
return(res);
}
public static decimal[] GaussNewton(Func <decimal[], decimal[, ]> J, Func <decimal[], decimal[]> r, decimal[] p0)
{
decimal[] p = p0;
decimal a = 1;
int iter = 0;
while (iter < 100) //TODO: while error is large
{
decimal[] pAdd = LinearLeastSquares(J(p), MathDecimal.Negative(r(p)));
int innerIter = 0;
a = 1;
while (MathDecimal.Pow2(MathDecimal.Norm2(r(p))) - MathDecimal.Pow2(MathDecimal.Norm2(MathDecimal.Sum(p, MathDecimal.Prod(a, pAdd)))) <
1M / 2M * a * MathDecimal.Pow2(MathDecimal.Norm2(MathDecimal.Prod(J(p), p))) && innerIter < 50)
{
a /= 2;
innerIter++;
}
int s = 0;
p = MathDecimal.Sum(p, MathDecimal.Prod(a, pAdd));
iter++;
}
return(p);
}
public static decimal[] LinearLeastSquares(decimal[,] A, decimal[] b, String method = "SVD")
{
decimal[] res = new decimal[A.GetLength(1)];
if (method == "Gauss")
{
decimal[,] B = MathDecimal.Prod(MathDecimal.Transpose(A), A);
decimal[] y = MathDecimal.Prod(MathDecimal.Transpose(A), b);
res = LinearAlgebra.Gauss(B, y);
}
else if (method == "LUDecomposition")
{
List <decimal[, ]> LU = LinearAlgebra.LUDecomposition(A);// L, U, PI1, Pi2;
//Solve system Ly=b1, where b1=PI1*b
//L^T*L*y=L^T*b1
// M=L^T * L , N=L^T*PI1*b
decimal[,] M = MathDecimal.Prod(MathDecimal.Transpose(LU[0]), LU[0]);
decimal[] N = MathDecimal.Prod(MathDecimal.Transpose(LU[0]), MathDecimal.Prod(LU[2], b));
decimal[] y = LinearAlgebra.Gauss(M, N);
//Solve system y=U*x1=U*PI2^T*x
res = MathDecimal.Prod(LU[3], LinearAlgebra.BackwardSubstitutionUpp(LU[1], y));
}
else if (method == "SVD")
{
double[,] ADouble = new double[A.GetLength(0), A.GetLength(1)];
for (int i = 0; i < A.GetLength(0); i++)
{
for (int j = 0; j < A.GetLength(1); j++)
{
ADouble[i, j] = (double)A[i, j];
}
}
Matrix <double> JMatrix = DenseMatrix.OfArray(ADouble);
MathNet.Numerics.LinearAlgebra.Factorization.Svd <double> Jsvd = JMatrix.Svd();
double[,] LDouble = Jsvd.W.ToArray();
double[,] UDouble = Jsvd.U.ToArray();
double[,] VtDouble = Jsvd.VT.ToArray();
decimal[,] L = new decimal[A.GetLength(0), A.GetLength(1)];
decimal[,] U = new decimal[A.GetLength(0), A.GetLength(0)];
decimal[,] Vt = new decimal[A.GetLength(1), A.GetLength(1)];
for (int i = 0; i < A.GetLength(0); i++)
{
for (int j = 0; j < A.GetLength(1); j++)
{
L[i, j] = (decimal)LDouble[i, j];
}
}
for (int i = 0; i < A.GetLength(0); i++)
{
for (int j = 0; j < A.GetLength(0); j++)
{
U[i, j] = (decimal)UDouble[i, j];
}
}
for (int i = 0; i < A.GetLength(1); i++)
{
for (int j = 0; j < A.GetLength(1); j++)
{
Vt[i, j] = (decimal)VtDouble[i, j];
}
}
for (int i = 0; i < L.GetLength(1); i++)
{
if (L[i, i] < 0.0000000000001M && L[i, i] > -0.0000000000001M)
{
L[i, i] = 0;
}
else
{
L[i, i] = 1 / L[i, i];
}
}
res = MathDecimal.Prod(MathDecimal.Transpose(Vt), MathDecimal.Prod(MathDecimal.Transpose(L), MathDecimal.Prod(MathDecimal.Transpose(U), b)));
}
return(res);
}
public static List <decimal[, ]> LUDecomposition(decimal[,] A)
{
int rows = A.GetLength(0);
int cols = A.GetLength(1);
decimal l, temp;
int maxRowIndex, maxColIndex;
List <decimal[, ]> result = new List <decimal[, ]>();
decimal[,] L = new decimal[rows, cols];
decimal[,] Alocal = new decimal[rows, cols];
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < cols; j++)
{
Alocal[i, j] = A[i, j];
}
}
for (int i = 0; i < cols; i++)
{
L[i, i] = 1;
}
decimal[,] Pi1 = new decimal[rows, rows];
for (int i = 0; i < rows; i++)
{
Pi1[i, i] = 1;
}
decimal[,] Pi2 = new decimal[cols, cols];
for (int i = 0; i < cols; i++)
{
Pi2[i, i] = 1;
}
for (int i = 0; i < cols; i++)
{
//Find the largest element in the i-th column
maxRowIndex = i;
maxColIndex = i;
for (int j = i + 1; j < rows; j++)
{
for (int k = i; k < cols; k++)
{
if (MathDecimal.Abs(Alocal[maxRowIndex, maxColIndex]) < MathDecimal.Abs(Alocal[j, k]))
{
maxRowIndex = j;
maxColIndex = k;
}
}
}
//Exchange rows
for (int j = i; j < cols; j++)
{
temp = Alocal[i, j];
Alocal[i, j] = Alocal[maxRowIndex, j];
Alocal[maxRowIndex, j] = temp;
}
for (int j = 0; j < i; j++)
{
temp = L[i, j];
L[i, j] = L[maxRowIndex, j];
L[maxRowIndex, j] = temp;
}
for (int j = 0; j < rows; j++)
{
temp = Pi1[i, j];
Pi1[i, j] = Pi1[maxRowIndex, j];
Pi1[maxRowIndex, j] = temp;
}
//Exchange columns
for (int j = 0; j < rows; j++)
{
temp = Alocal[j, i];
Alocal[j, i] = Alocal[j, maxColIndex];
Alocal[j, maxColIndex] = temp;
}
for (int j = 0; j < cols; j++)
{
temp = Pi2[j, i];
Pi2[j, i] = Pi2[j, maxColIndex];
Pi2[j, maxColIndex] = temp;
}
//Proceed with elimination
for (int j = i + 1; j < rows; j++)
{
l = Alocal[j, i] / Alocal[i, i];
L[j, i] = l;
Alocal[j, i] = 0;
for (int k = i + 1; k < cols; k++)
{
Alocal[j, k] = Alocal[j, k] - l * Alocal[i, k];
}
}
}
decimal[,] U = new decimal[cols, cols];
for (int i = 0; i < cols; i++)
{
for (int j = 0; j < cols; j++)
{
U[i, j] = Alocal[i, j];
}
}
result.Add(L);
result.Add(U);
result.Add(Pi1);
result.Add(Pi2);
return(result);
}
public static decimal Norm2(decimal[] x)
{
return(MathDecimal.Sqrt(SquaredNorm2(x)));
}
