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