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