Accord.Math.Decompositions.QrDecomposition.Solve C# (CSharp) Method

        public double[,] Solve(double[,] value)
        {
            if (value == null)
                throw new ArgumentNullException("value", "Matrix cannot be null.");

            if (value.GetLength(0) != qr.GetLength(0))
                throw new ArgumentException("Matrix row dimensions must agree.");

            if (!FullRank)
                throw new InvalidOperationException("Matrix is rank deficient.");

            // Copy right hand side
            int count = value.GetLength(1);
            var X = (double[,]) value.Clone();
            int m = qr.GetLength(0);
            int n = qr.GetLength(1);

            // Compute Y = transpose(Q)*B
            for (int k = 0; k < n; k++)
            {
                for (int j = 0; j < count; j++)
                {
                    double s = 0.0;

                    for (int i = k; i < m; i++)
                        s += qr[i, k]*X[i, j];

                    s = -s/qr[k, k];

                    for (int i = k; i < m; i++)
                        X[i, j] += s*qr[i, k];
                }
            }

            // Solve R*X = Y;
            for (int k = n - 1; k >= 0; k--)
            {
                for (int j = 0; j < count; j++)
                    X[k, j] /= Rdiag[k];

                for (int i = 0; i < k; i++)
                    for (int j = 0; j < count; j++)
                        X[i, j] -= X[k, j]*qr[i, k];
            }

            var r = new double[n,count];
            for (int i = 0; i < n; i++)
                for (int j = 0; j < count; j++)
                    r[i, j] = X[i, j];

            return r;
        }

        public void QrDecompositionConstructorTest()
        {
            double[,] value =
            {
               {  2, -1,  0 },
               { -1,  2, -1 },
               {  0, -1,  2 }
            };


            QrDecomposition target = new QrDecomposition(value);

            // Decomposition Identity
            var Q = target.OrthogonalFactor;
            var QQt = Q.Multiply(Q.Transpose());
            Assert.IsTrue(Matrix.IsEqual(QQt, Matrix.Identity(3), 0.0000001));


            // Linear system solving
            double[,] B = Matrix.ColumnVector(new double[] { 1, 2, 3 });
            double[,] expected = Matrix.ColumnVector(new double[] { 2.5, 4.0, 3.5 });
            double[,] actual = target.Solve(B);

            Assert.IsTrue(Matrix.IsEqual(expected, actual, 0.0000000000001));
        }