GNSS伪距测量

qq_54401811

已于 2023-07-28 22:04:56 修改

阅读量212

点赞数

文章标签： java 算法开发语言

于 2023-07-27 17:10:51 首次发布

本文链接：https://blog.csdn.net/qq_54401811/article/details/131964341

版权

一、Datacenter.cs

1.1 sat卫星数据类

包含txt文件中的单个卫星的：PRN号、空间坐标、卫星钟差、高度角、伪距、对流层延迟以及站星估计几何距离R0。

/// <summary>
    /// 卫星类
    /// </summary>
    public class Sat
    {
        public Matrix stapos;       //卫星位置
        public string PRN;          //卫星PRN
        public double satColck;     //卫星钟差
        public double elevation;    //卫星高度角
        public double cl;           //伪距
        public double tropDely;     //对流层延迟
        public double R0;           //估计几何距离
    }

1.2 Epoch历元数据类

题目要求的是计算每个历元的平差数据。

包含txt文件中单个历元的：卫星数、历元GPS时间、历元内sat卫星类列表、最小二乘求得的坐标增量dx、最小二乘估计的位置、单位权中误差、各个方向的中误差、PDOP、协因数阵Q、权阵P、系数矩阵B、观测残差向量L、验后残差V。

/// <summary>
    /// 历元类
    /// </summary>
    public class Epoch
    {
        public int satNum;      //卫星数
        public int gpsTime;     //历元时间
        public List<Sat> sats;  //卫星观测值列表
        public Matrix dx;       //最小二乘求得的增量dx
        public Matrix pos;      //最小二乘估计的位置
        public double sigma0;   //延后单位权中误差
        public Matrix sigma;    //各个方向的中误差
        public double PDOP;     //PDOP
        public Matrix Q;        //协因数阵
        public Matrix P;        //权阵
        public Matrix B;        //设计矩阵
        public Matrix L;        //观测残差向量
        public Matrix V;        //验后残差
    }

1.3 DataCenter数据中心

用于储存：测站近似坐标（计算R0）、文件所有Epoch历元列表。

/// <summary>
    /// 数据中心类
    /// </summary>
    public class DataCenter
    {
        public List<Epoch> Epoches;     //历元列表
        public Matrix APPROX_POSITION;  //近似坐标
    }

1.4 Matirx矩阵类

用于声明、储存以及计算（加、减、乘、逆、转置）矩阵

 /// <summary>
    /// 矩阵类
    /// </summary>
    public class Matrix
    {
        public int m;
        public int n;
        public double[,] arr;

        /// <summary>
        /// 创建一个矩阵0*0
        /// </summary>
        public Matrix()
        {
            m = 0;
            n = 0;
            arr = new double[m, n];
        }

        /// <summary>
        /// 拷贝构造
        /// </summary>
        /// <param name="s"></param>
        public Matrix(Matrix s)
        {
            this.m = s.m;
            this.n = s.n;
            arr = new double[m, n];
            this.arr = s.arr;
        }

        public Matrix(int mm, int nn, double[,] arr)
        {
            m = mm;
            n = nn;
            this.arr = arr;
        }

        public Matrix(int mm, int nn)
        {
            m = mm;
            n = nn;
            arr = new double[m, n];
        }

        /// <summary>
        /// 创建单位阵
        /// </summary>
        /// <param name="mm"></param>
        /// <param name="nn"></param>
        /// <returns></returns>
        public Matrix MatrixE(int mm, int nn)
        {
            Matrix matrix = new Matrix(mm, nn);
            m = mm;
            n = nn;
            arr = new double[m, n];

            for (int i = 0; i < m; i++)
            {
                for (int j = 0; j < n; j++)
                {
                    arr[i, j] = 1;
                }
            }
            return matrix;
        }

        /// <summary>
        /// 重载操作符实现矩阵加法
        /// </summary>
        /// <param name="A"></param>
        /// <param name="B"></param>
        /// <returns></returns>
        static public Matrix operator +(Matrix A, Matrix B)
        {
            Matrix C = new Matrix(A.m, A.n);
            //判断是否可以运算    
            if (A.m != B.m || A.n != B.n || A.m != C.m || A.n != C.n)
            {
                System.Windows.Forms.MessageBox.Show("矩阵维数不同");
            }
            for (int i = 0; i < C.m; i++)
            {
                for (int j = 0; j < C.n; j++)
                {
                    C.arr[i, j] = A.arr[i, j] + B.arr[i, j];
                }
            }

            return C;
        }

        /// <summary>
        /// 重载操作符实现矩阵减法
        /// </summary>
        /// <param name="A"></param>
        /// <param name="B"></param>
        /// <returns></returns>
        static public Matrix operator -(Matrix A, Matrix B)
        {
            int i = 0;
            int j = 0;
            Matrix C = new Matrix(A.m, B.n);
            //判断是否可以运算    
            if (A.m != B.m || A.n != B.n ||
                A.m != C.m || A.n != C.n)
            {
                Console.ReadKey();
            }
            for (i = 0; i < C.m; i++)
            {
                for (j = 0; j < C.n; j++)
                {
                    C.arr[i, j] = A.arr[i, j] - B.arr[i, j];
                }
            }
            return C;
        }

        /// <summary>
        /// 重载操作符实现矩阵乘法
        /// </summary>
        /// <param name="A"></param>
        /// <param name="B"></param>
        /// <returns></returns>
        static public Matrix operator *(Matrix A, Matrix B)
        {
            int i = 0;
            int j = 0;
            int k = 0;
            double temp = 0;
            Matrix C = new Matrix(A.m, B.n);
            //判断是否可以运算    
            if (A.m != C.m || B.n != C.n ||
                A.n != B.m)
            {
                return C;
            }
            //运算    
            for (i = 0; i < C.m; i++)
            {
                for (j = 0; j < C.n; j++)
                {
                    temp = 0;
                    for (k = 0; k < A.n; k++)
                    {
                        temp += A.arr[i, k] * B.arr[k, j];
                    }
                    C.arr[i, j] = temp;
                }
            }
            return C;
        }

        /// <summary>
        /// 矩阵转置
        /// </summary>
        /// <param name="A"></param>
        /// <returns></returns>
        public Matrix transposs(Matrix A)
        {
            int i = 0;
            int j = 0;
            Matrix B = new Matrix(A.n, A.m);
            for (i = 0; i < B.m; i++)
            {
                for (j = 0; j < B.n; j++)
                {
                    B.arr[i, j] = A.arr[j, i];
                }
            }
            return B;
        }

        public double[,] InverseMatrix(double[,] matrix)
        {
            int n = matrix.GetLength(0);
            double[,] result = new double[n, n];
            double[,] temp = new double[n, 2 * n];

            //将矩阵和单位矩阵拼接成一个2n*n的矩阵
            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < n; j++)
                {
                    temp[i, j] = matrix[i, j];
                    temp[i, j + n] = i == j ? 1 : 0;
                }
            }
            //高斯-约旦消元法
            for (int i = 0; i < n; i++)
            {
                double tempValue = temp[i, i];
                for (int j = i; j < 2 * n; j++)
                {
                    temp[i, j] /= tempValue;
                }
                for (int j = 0; j < n; j++)
                {
                    if (j != i)
                    {
                        tempValue = temp[j, i];
                        for (int k = i; k < 2 * n; k++)
                        {
                            temp[j, k] -= tempValue * temp[i, k];
                        }
                    }
                }
            }
            //取出逆矩阵
            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < n; j++)
                {
                    result[i, j] = temp[i, j + n];
                }
            }

            return result;
        }

        /// <summary>
        /// 矩阵求逆
        /// </summary>
        /// <param name="matrix"></param>
        /// <returns></returns>
        public Matrix Inverse(Matrix matrix)
        {
            matrix.arr = InverseMatrix(matrix.arr);
            return matrix;
        }


    }