摄影外方位元素求解，结果为非数字，跪求高手

Question

using System;
using System.Collections.Generic;
using System.Text;namespace ConsoleApplication8
{
    class Program
    {
        public class data
        {
            double Xs, Ys, Zs, o=0, w=0, k=0;
            double Xs0, Ys0, Zs0, m = 3000, f = 0.150;
            //控制点坐标已给
            double[] X = new double[3] { 100000.00, 100000.00, 106000.00 };
            double[] Y = new double[3] { 137500.00, 142500.00, 137500.00 };
            double[] Z = new double[3] { 11.00, 36.00, 42.00};
            //对应像点坐标
            double[] x = new double[3] { -91.596, -94.230, 95.207 };
            double[] y = new double[3] { -74.859, 81.446, -75.521 };            double[] _x = new double[3];
            double[] _y = new double[3];
            double[] _z = new double[3];
            //各矩阵
            double[,] A = new double[6, 12];
            double[,] M_A = new double[6, 6];
            double[,] M_AA = new double[6, 6];
            double[,] M_AAA = new double[6, 6];
            double[,] M_AAAA = new double[6, 6];
            double[,] M_AT = new double[6, 6];
            double[,] R = new double[6, 1];
            double[,] L = new double[6, 1];
            double a1, a2, a3, b1, b2, b3, c1, c2, c3;
            double p, q;            int  i, j;            public void input()
            {
                for (i = 0; i < 3; i++)
                {
                    p += X[i];
                    q += Y[i];
                }
                //初值
                Zs0 = m * f;
                Xs0 = p / 3;
                Ys0 = q / 3;
            }
            public void M()
            {                               a1 = Math.Cos(o) * Math.Cos(k) - Math.Sin(o) * Math.Sin(w) * Math.Sin(k);
                a2 = -Math.Cos(o) * Math.Sin(k) - Math.Sin(o) * Math.Sin(w) * Math.Cos(k);
                a3 = -Math.Sin(o) * Math.Cos(w);
                b1 = Math.Cos(w) * Math.Sin(k);
                b2 = Math.Cos(w) * Math.Cos(k);
                b3 = -Math.Sin(w);
                c1 = Math.Sin(o) * Math.Cos(k) + Math.Cos(o) * Math.Sin(w) * Math.Sin(k);
                c2 = -Math.Sin(o) * Math.Sin(k) + Math.Cos(o) * Math.Sin(w) * Math.Cos(k);
                c3 = Math.Cos(o) * Math.Cos(w);
                //求系数
                for (i = 0; i < 3; i++)
                {
                    _x[i] = (-f) * a1 * (X[i] - Xs0) + b1 * (Y[i] - Ys0) + c1 * (Z[i] - Zs0) / a3 * (X[i] - Xs0) + b3 * (Y[i] - Ys0) + c3 * (Z[i] - Zs0);
                    _y[i] = (-f) * a2 * (X[i] - Xs0) + b2 * (Y[i] - Ys0) + c2 * (Z[i] - Zs0) / a3 * (X[i] - Xs0) + b3 * (Y[i] - Ys0) + c3 * (Z[i] - Zs0);
                    _z[i] = a3 * (X[i] - Xs0) + b3 * (Y[i] - Ys0) + c3 * (Z[i] - Zs0);
                    M_A[i * 2, 0] = (a1 * f + a3 * x[i]) / _z[i];
                    M_A[i * 2, 1] = (b1 * f + b3 * x[i]) / _z[i];
                    M_A[i * 2, 2] = (c1 * f + c3 * x[i]) / _z[i];
                    M_A[i * 2, 3] = y[i] * Math.Sin(w) - x[i] / f * (x[i] * Math.Cos(k) - y[i] * Math.Sin(k)) + f * Math.Cos(k) * Math.Cos(w);
                    M_A[i * 2, 4] = -f * Math.Sin(k) - x[i] / f * (x[i] * Math.Sin(k) + y[i] * Math.Cos(k));
                    M_A[i * 2, 5] = y[i];
                    M_A[i * 2 + 1, 0] = (a2 * f + a3 * x[i]) / _z[i];
                    M_A[i * 2 + 1, 1] = (b2 * f + b3 * x[i]) / _z[i];
                    M_A[i * 2 + 1, 2] = (c2 * f + c3 * x[i]) / _z[i];
                    M_A[i * 2 + 1, 3] = x[i] * Math.Sin(w) - x[i] / f * (x[i] * Math.Cos(k) - y[i] * Math.Sin(k)) + f * Math.Sin(k) * Math.Cos(w);
                    M_A[i * 2 + 1, 4] = -f * Math.Cos(k) - y[i] / f * (x[i] * Math.Sin(k) + y[i] * Math.Cos(k));
                    M_A[i * 2 + 1, 5] = -x[i];
                    L[i * 2, 0] = x[i] - _x[i];
                    L[i * 2 + 1, 0] = y[i] - _y[i];
                }
                //求A的转置
                for (i = 0; i < 6; i++)
                    for (j = 0; j < 6; j++)
                        M_AT[i, j] = M_A[j, i];
                //把A和AT的乘积放在M_AA中
                for (i = 0; i < 6; i++)
                    for (j = 0; j < 6; j++)
                        M_AA[i, j] += M_AT[i, j] * M_A[i, j];
                // 求M_AA的逆矩阵
                //定义A--临时数组                for (int s = 0; s < 6; s++)
                {
                    for (int t = 0; t < 12; t++)
                    {
                        A[s, t] = 0;
                    }
                }
                //把M_AA各值赋给A                for (int i = 0; i < 6; i++)
                {
                    for (int j = 0; j < 6; j++)
                    {
                        A[i, j] = M_AA[i, j];
                    }
                }                /**/
                ////在A加入初等方阵                for (int v = 0; v < 6; v++)
                {                    A[v, v + 6] = 1;
                }                //初等变换
                for (int s = 0; s < 6; s++)
                {                    if (A[s, s] != 1)
                    {                        double bs = A[s, s];
                        A[s, s] = 1;
                        for (int p = s + 1; p < 12; p++)
                        {
                            A[s, p] /= bs;
                        }
                    }
                    for (int q = 0; q < 6; q++)
                    {
                        if (q != s)
                        {
                            double bs = A[q, s];
                            for (int p = 0; p < 12; p++)
                            {
                                A[q, p] -= bs * A[s, p];
                            }
                        }
                        else
                        {
                            continue;
                        }
                    }
                }
                //把A的值赋给M_AAA
                for (i = 0; i < 6; i++)
                    for (j = 0; j < 6; j++)
                        M_AAA[i, j] = A[i, j];
                for (i = 0; i < 6; i++)
                    for (j = 0; j <6; j++)
                        M_AAAA[i, j] += M_AAA[i, j] * M_AT[j, i];
                for (i = 0; i < 6; i++)
                    for (j = 0; j < 1; j++)
                    {
                        R[i, j] = 0;
                        for (int v = 0; v < 6; v++)
                        {
                            R[i, j] += M_AAAA[i, v] * L[v, j];
 
                        }
 
                    }
                       
                //外方位元素如下
                Xs = R[0, 0];
                Ys = R[1, 0];
                Zs = R[2, 0];
                o = R[3, 0];
                w = R[4, 0];
                k = R[5, 0];
            }
            public void output()
            {
                Console.WriteLine("外方位元素如下：");
                Console.WriteLine("Xs={0}", Xs);
                Console.WriteLine("Ys={0}", Ys);
                Console.WriteLine("Zs={0}", Zs);
                Console.WriteLine("o={0}", o);
                Console.WriteLine("w ={0}", w);
                Console.WriteLine("k={0}", k);
                Console.ReadLine();            }        }
        static void Main(string[] args)
        {
            data d = new data();
            d.input();
            d.M();
            d.output();
        }
    }
}