private void computeaccCalButton_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);
}
accAlignment00.Text = calMat[0, 0].ToString();
accAlignment01.Text = calMat[0, 1].ToString();
accAlignment02.Text = calMat[0, 2].ToString();
accAlignment10.Text = calMat[1, 0].ToString();
accAlignment11.Text = calMat[1, 1].ToString();
accAlignment12.Text = calMat[1, 2].ToString();
accAlignment20.Text = calMat[2, 0].ToString();
accAlignment21.Text = calMat[2, 1].ToString();
accAlignment22.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;
accAlignmentCommitButton.Enabled = true;
}
public static void Main(System.String[] argv)
{
/*
| Tests LU, QR, SVD and symmetric Eig decompositions.
|
| n = order of magic square.
| trace = diagonal sum, should be the magic sum, (n^3 + n)/2.
| max_eig = maximum eigenvalue of (A + A')/2, should equal trace.
| rank = linear algebraic rank,
| should equal n if n is odd, be less than n if n is even.
| cond = L_2 condition number, ratio of singular values.
| lu_res = test of LU factorization, norm1(L*U-A(p,:))/(n*eps).
| qr_res = test of QR factorization, norm1(Q*R-A)/(n*eps).
*/
print("\n Test of GeneralMatrix Class, using magic squares.\n");
print(" See MagicSquareExample.main() for an explanation.\n");
print("\n n trace max_eig rank cond lu_res qr_res\n\n");
System.DateTime start_time = System.DateTime.Now;
double eps = System.Math.Pow(2.0, - 52.0);
for (int n = 3; n <= 32; n++)
{
print(fixedWidthIntegertoString(n, 7));
GeneralMatrix M = magic(n);
//UPGRADE_WARNING: Narrowing conversions may produce unexpected results in C#. 'ms-help://MS.VSCC.2003/commoner/redir/redirect.htm?keyword="jlca1042"'
int t = (int) M.Trace();
print(fixedWidthIntegertoString(t, 10));
EigenvalueDecomposition E = new EigenvalueDecomposition(M.Add(M.Transpose()).Multiply(0.5));
double[] d = E.RealEigenvalues;
print(fixedWidthDoubletoString(d[n - 1], 14, 3));
int r = M.Rank();
print(fixedWidthIntegertoString(r, 7));
double c = M.Condition();
print(c < 1 / eps ? fixedWidthDoubletoString(c, 12, 3):" Inf");
LUDecomposition LU = new LUDecomposition(M);
GeneralMatrix L = LU.L;
GeneralMatrix U = LU.U;
int[] p = LU.Pivot;
GeneralMatrix R = L.Multiply(U).Subtract(M.GetMatrix(p, 0, n - 1));
double res = R.Norm1() / (n * eps);
print(fixedWidthDoubletoString(res, 12, 3));
QRDecomposition QR = new QRDecomposition(M);
GeneralMatrix Q = QR.Q;
R = QR.R;
R = Q.Multiply(R).Subtract(M);
res = R.Norm1() / (n * eps);
print(fixedWidthDoubletoString(res, 12, 3));
print("\n");
}
System.DateTime stop_time = System.DateTime.Now;
double etime = (stop_time.Ticks - start_time.Ticks) / 1000.0;
print("\nElapsed Time = " + fixedWidthDoubletoString(etime, 12, 3) + " seconds\n");
print("Adios\n");
}
