GPS伪距单点定位

GPS第三次作业

这次就没什么难度了，只是照着抄公式而已，还好有聪神帮忙解读。

截图如下：

唯一有难度的就是矩阵的运算了，还好有人写过直接拿来用。。。

但是木有逆矩阵的计算，我又东凑西凑的找了个，还是不错的，分享下。。。

public Matrix Inverse(double[,] a, int n)    //求n阶方阵a的逆矩阵(Gauss算法)
        {
            double[,] UnitMatri=new double[n,n];//定义一个单位矩阵
            int i = 0,j;
            double temp;
            for (i = 0; i < n; i++)
            {
                UnitMatri[i, i] = 1;
            }
            for(i=0;i<n;i++)
            {
                int maxline = MaxLine(a, i, n);
                for (j = 0; j < n; j++)
                {
                    temp = a[i, j];
                    a[i, j] = a[maxline, j];
                    a[maxline, j] = temp;

                temp = UnitMatri[i, j];
                UnitMatri[i, j] = UnitMatri[maxline, j];
                UnitMatri[maxline, j] = temp;
            }
            double max = a[i, i];
            for (int k = i + 1; k &lt; n; k++)
            {
                double r = a[k, i] / max;
                for (int m = 0; m &lt; n; m++)
                {
                    a[k, m] = a[k, m] - a[i, m] * r;
                    UnitMatri[k, m] = UnitMatri[k, m] - UnitMatri[i, m] * r;
                }
            }

        }
        for (int s = n - 1; s &gt;= 0; s--)
        {
            double maxnew = a[s, s];
            for (i = s; i &lt; n; i++)
                a[s, i] = a[s, i] / maxnew;
            for (i = 0; i &lt; n; i++)
                UnitMatri[s, i] = UnitMatri[s,i]/ maxnew;
            for (i = s - 1; i &gt;= 0; i--)
            {
                maxnew = a[i, s];
                //a[i, s] = 0;
                for (j = 0; j &lt; n; j++)
                {
                    UnitMatri[i, j] = UnitMatri[i, j] - maxnew * UnitMatri[s, j];
                }
            }
        }
        Matrix t = new Matrix(UnitMatri);
        return t;
    }</pre>