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))); }