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