AHRSInterface.AccCal.computeaccCalButton_Click C# (CSharp) Method

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