private void computeMagCalButton_Click(object sender, EventArgs e)
{
int i,j;
calStatusText.Text = "Computing Calibration...";
// Construct D matrix
// D = [x.^2, y.^2, z.^2, x.*y, x.*z, y.*z, x, y, z, ones(N,1)];
for (i = 0; i < SAMPLES; i++ )
{
// x^2 term
D.SetElement(i,0, loggedData[i,0]*loggedData[i,0]);
// y^2 term
D.SetElement(i,1,loggedData[i,1]*loggedData[i,1]);
// z^2 term
D.SetElement(i, 2, loggedData[i, 2] * loggedData[i, 2]);
// x*y term
D.SetElement(i,3,loggedData[i,0]*loggedData[i,1]);
// x*z term
D.SetElement(i,4,loggedData[i,0]*loggedData[i,2]);
// y*z term
D.SetElement(i,5,loggedData[i,1]*loggedData[i,2]);
// x term
D.SetElement(i,6,loggedData[i,0]);
// y term
D.SetElement(i,7,loggedData[i,1]);
// z term
D.SetElement(i,8,loggedData[i,2]);
// Constant term
D.SetElement(i,9,1);
}
// QR=triu(qr(D))
QRDecomposition QR = new QRDecomposition(D);
// [U,S,V] = svd(D)
SingularValueDecomposition SVD = new SingularValueDecomposition(QR.R);
GeneralMatrix V = SVD.GetV();
GeneralMatrix A = new GeneralMatrix(3, 3);
double[] p = new double[V.RowDimension];
for (i = 0; i < V.RowDimension; i++ )
{
p[i] = V.GetElement(i,V.ColumnDimension-1);
}
/*
A = [p(1) p(4)/2 p(5)/2;
p(4)/2 p(2) p(6)/2;
p(5)/2 p(6)/2 p(3)];
*/
if (p[0] < 0)
{
for (i = 0; i < V.RowDimension; i++)
{
p[i] = -p[i];
}
}
A.SetElement(0,0,p[0]);
A.SetElement(0,1,p[3]/2);
A.SetElement(1,2,p[4]/2);
A.SetElement(1,0,p[3]/2);
A.SetElement(1,1,p[1]);
A.SetElement(1,2,p[5]/2);
A.SetElement(2,0,p[4]/2);
A.SetElement(2,1,p[5]/2);
A.SetElement(2,2,p[2]);
CholeskyDecomposition Chol = new CholeskyDecomposition(A);
GeneralMatrix Ut = Chol.GetL();
GeneralMatrix U = Ut.Transpose();
double[] bvect = {p[6]/2,p[7]/2,p[8]/2};
double d = p[9];
GeneralMatrix b = new GeneralMatrix(bvect,3);
GeneralMatrix v = Ut.Solve(b);
double vnorm_sqrd = v.GetElement(0,0)*v.GetElement(0,0) + v.GetElement(1,0)*v.GetElement(1,0) + v.GetElement(2,0)*v.GetElement(2,0);
double s = 1/Math.Sqrt(vnorm_sqrd - d);
GeneralMatrix c = U.Solve(v);
for (i = 0; i < 3; i++)
{
c.SetElement(i, 0, -c.GetElement(i, 0));
}
U = U.Multiply(s);
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
calMat[i, j] = U.GetElement(i, j);
}
}
for (i = 0; i < 3; i++)
{
bias[i] = c.GetElement(i, 0);
}
magAlignment00.Text = calMat[0, 0].ToString();
magAlignment01.Text = calMat[0, 1].ToString();
magAlignment02.Text = calMat[0, 2].ToString();
magAlignment10.Text = calMat[1, 0].ToString();
magAlignment11.Text = calMat[1, 1].ToString();
magAlignment12.Text = calMat[1, 2].ToString();
magAlignment20.Text = calMat[2, 0].ToString();
magAlignment21.Text = calMat[2, 1].ToString();
magAlignment22.Text = calMat[2, 2].ToString();
biasX.Text = bias[0].ToString();
biasY.Text = bias[1].ToString();
biasZ.Text = bias[2].ToString();
calStatusText.Text = "Done";
flashCommitButton.Enabled = true;
magAlignmentCommitButton.Enabled = true;
}