예제 #1
0
        /*矩阵减运算*/
        public static matrix operator -(matrix a, matrix b)
        {
            int row = a.rows, column = a.columns;

            if (row == b.rows && column == b.columns)
            {
                matrix c = new matrix(row, column);
                for (int i = 1; i <= row; i++)
                {
                    for (int j = 1; j <= column; j++)
                    {
                        c[i, j] = a[i, j] - b[i, j];
                    }
                }
                return(c);
            }
            else
            {
                return(null);
            }
        }
예제 #2
0
        public void error_enu(List <result> res)
        {
            matrix xyz = new matrix(3, 1);
            matrix r   = new matrix(3, 1);
            matrix enu = new matrix(3, 1);

            if (realX != null && realY != null && realZ != null)
            {
                xyz[1, 1] = double.Parse(realX.Text.Trim());
                xyz[2, 1] = double.Parse(realY.Text.Trim());
                xyz[3, 1] = double.Parse(realZ.Text.Trim());
                foreach (var v in res)
                {
                    r[1, 1] = v.X - xyz[1, 1];
                    r[2, 1] = v.Y - xyz[2, 1];
                    r[3, 1] = v.Z - xyz[3, 1];
                    transcoor.xyz2enu(xyz, r, enu);
                    v.error_E = enu[2, 1];
                    v.error_N = enu[1, 1];
                    v.error_U = enu[3, 1];
                }
            }
        }
예제 #3
0
        public static int res_ppp(List <obs_s> obs, station sta, ppp_t p3, int n, double[][] rs, double[][] dts, double[][] azel, double[] vare, erp_t erp,
                                  dcb_t dcb, double[] x, double[] R, double[] v, matrix H, int[] svh)
        {
            double r, dtrp, vart = Math.Pow(0.01, 2), elmin = 15 * D2R;

            double[] rr = new double[3], disp = new double[3], pos = new double[3], e = new double[3], meas = new double[2], varm = new double[2];
            double[] dtdx = new double[3], dantr = new double[3], var = new double[nx * 2], dants = new double[2], phw = new double[1];
            matrix   pos_ = new matrix(3, 1);
            int      i, j, nv = 0, k, sat;

            for (i = 0; i < 3; i++)
            {
                rr[i] = x[i];
            }

            /* earth tides correction */ //地球潮汐改正 固体潮
            tidedisp(obs[0].t, rtklibcmn.gpst2utc(obs[0].rtkt), rr, erp, disp);
            for (i = 0; i < 3; i++)
            {
                rr[i] += disp[i];
            }
            transcoor.ecef2pos(matrix.Array2matrix(rr), pos_);
            for (i = 0; i < 3; i++)
            {
                pos[i] = pos_[i + 1, 1];
            }

            for (i = 0; i < 32; i++)
            {
                p3.vsat[i] = 0;
            }

            for (i = 0; i < n; i++)
            {
                sat = int.Parse(obs[i].sprn.Substring(1, 2));

                //
                if (p3.spp.vsat[sat - 1] == 0 || svh[i] < 0)
                {
                    continue;
                }
                if ((r = geodist(rs[i], rr, e)) <= 0 || pppcmn.satel(rr, rs[i], azel[i]) < elmin)
                {
                    continue;
                }

                dtrp = pppcmn.prectrop(obs[i].t, pos, azel[i], x[4], dtdx); //精密对流层模型

                pppcmn.satanxpcv(rs[i], rr, pcv, dants, obs[i].sprn);       //卫星天线相位偏差,返回每个频率的改正值
                pppcmn.antxmodel(pcv, sta.atxdel, azel[i], dantr, sta.anxtype);
                pppcmn.windup(obs[i].t, p3.soltime, rs[i], rr, phw);

                if (corrmens(obs[i], dcb, pos, dantr, dants, phw, meas, varm) != 1)
                {
                    continue;
                }
                /* satellite clock and tropospheric delay */ //卫星钟差和电离层延迟
                r += -CLIGHT * dts[i][0] + dtrp;

                for (j = 0; j < 2; j++)
                {
                    if (meas[j] == 0)
                    {
                        continue;
                    }
                    v[nv] = meas[j] - r;
                    for (k = 0; k < nx; k++)
                    {
                        H[nv + 1, k + 1] = 0.0;
                    }
                    for (k = 0; k < 3; k++)
                    {
                        H[nv + 1, k + 1] = -e[k];
                    }
                    v[nv]       -= x[3]; H[nv + 1, 4] = 1.0;
                    H[nv + 1, 5] = dtdx[0];

                    if (j == 0)
                    {
                        v[nv] -= x[4 + sat];
                        H[nv + 1, 5 + sat] = 1.0;
                    }
                    var[nv] = varm[j] + vare[i] + vart + varerr(azel[i][1], j);

                    if (Math.Abs(v[nv]) > 30)
                    {
                        continue;
                    }

                    if (j == 0)
                    {
                        p3.vsat[sat - 1] = 1;
                    }
                    nv++;
                }
            }
            for (i = 0; i < nv; i++)
            {
                R[i] = var[i];
            }

            return(nv);
        }
예제 #4
0
        //obs均已按照PRN号大小排列好
        public static int estpos(spp_t spp, List <obs_s> obs, dcb_t dcb, nav_t nav, int n, double[][] rs, double[][] dts,
                                 double[] vare, double[][] azel, int[] vsat, int[] svh)
        {
            double[] v_ = new double[n], x = new double[4], var = new double[n];
            matrix   H_ = new matrix(n, 4), H, v, dx, Q, P;
            int      nv;
            double   sig;
            int      i, j, k, stat = 1;

            for (i = 0; i < 3; i++)
            {
                x[i] = spp.rr[i];
            }

            for (i = 0; i < 10; i++)//迭代
            {
                nv = rescod(i, obs, nav, n, rs, dts, vare, azel, vsat, dcb, x, H_, v_, var, svh);
                if (nv < 4)
                {
                    break;        //nv是参与解算的实际卫星数
                }
                H = new matrix(nv, 4); v = new matrix(nv, 1); P = new matrix(nv, nv);
                for (j = 0; j < nv; j++)
                {
                    sig         = Math.Sqrt(var[j]);
                    v[j + 1, 1] = v_[j] / sig;
                    for (k = 1; k <= 4; k++)
                    {
                        H[j + 1, k] = H_[j + 1, k] / sig;
                    }
                }
                dx = matrix.inv(matrix.transp(H) * H) * matrix.transp(H) * v;
                Q  = matrix.transp(H) * H;

                for (j = 0; j < 4; j++)
                {
                    x[j] += dx[j + 1, 1];
                }

                if (Math.Sqrt(dx[1, 1] * dx[1, 1] + dx[2, 1] * dx[2, 1] + dx[3, 1] * dx[3, 1] + dx[4, 1] * dx[4, 1]) < 1E-4)
                {
                    spp.dtr = x[3] / CLIGHT;
                    for (j = 0; j < 3; j++)
                    {
                        spp.rr[j] = x[j];
                    }
                    spp.tcurr.gpsec = obs[0].t.gpsec - spp.dtr;
                    spp.tcur        = rtklibcmn.timeadd(obs[0].rtkt, -x[3] / CLIGHT);
                    for (j = 0; j < 6; j++)
                    {
                        spp.tcurr.calend[j] = obs[0].t.calend[j];
                    }

                    stat = valsol(azel, vsat, 15.0 * D2R, n, v, nv, 4);
                    return(stat);
                }
            }


            return(1);
        }
예제 #5
0
        public static void upbias_ppp(ppp_t p3, List <obs_s> obs, dcb_t dcb)
        {
            double[] meas = new double[2], var = new double[2], bias = new double[32], pos = new double[3], rr = new double[3], phw = new double[1];
            double   offset = 0.0;
            matrix   pos_ = new matrix(3, 1);
            int      i, j, k, sat, n = obs.Count;

            for (i = 0; i < 32; i++)
            {
                p3.cyslip[i] = 0;
            }
            detslp_gf(p3, obs, n);
            /* reset phase-bias if expire obs outage counter */
            for (i = 0; i < 32; i++)
            {
                if (++p3.outc[i] > 5)
                {
                    initx(p3, 0.0, 0.0, i + 5);
                }
            }
            for (i = 0; i < 3; i++)
            {
                rr[i] = p3.spp.rr[i];
            }
            transcoor.ecef2pos(matrix.Array2matrix(rr), pos_);
            for (i = 0; i < 3; i++)
            {
                pos[i] = pos_[i + 1, 1];
            }


            for (i = k = 0; i < n; i++)
            {
                sat = int.Parse(obs[i].sprn.Substring(1, 2));
                j   = sat + 4;

                if ((corrmens(obs[i], dcb, pos, null, null, phw, meas, var)) != 1)
                {
                    continue;
                }
                ;

                bias[i] = meas[0] - meas[1];
                if (p3.x[j] == 0 || p3.cyslip[sat - 1] == 1)
                {
                    continue;
                }
                offset += bias[i] - p3.x[j];
                k++;
            }
            /* correct phase-code jump to enssure phase-code coherency */
            if (k >= 2 && Math.Abs(offset / k) > 0.0005 * CLIGHT)
            {
                for (i = 0; i < 32; i++)
                {
                    j = i + 5;
                    if (p3.x[j] != 0)
                    {
                        p3.x[j] += offset / k;
                    }
                }
            }
            for (i = 0; i < n; i++)
            {
                sat = int.Parse(obs[i].sprn.Substring(1, 2));
                j   = sat + 4;
                p3.P[j + 1, j + 1] += Math.Pow(1E-4, 2) * Math.Abs(p3.tt);
                if (p3.x[j] != 0 && p3.cyslip[sat - 1] != 1)
                {
                    continue;                                         //已初始化且不发生周跳
                }
                if (bias[i] == 0)
                {
                    continue;
                }
                initx(p3, bias[i], VAR_BIAS, j);
            }
        }
예제 #6
0
        public static void filter(matrix x, matrix P, matrix H, matrix R, matrix v, matrix xp, matrix Pp)
        {
            matrix Q   = H * P * matrix.transp(H) + R;
            matrix K   = P * matrix.transp(H) * matrix.inv(Q);
            matrix xp_ = new matrix(x.rows, x.columns);

            xp_ = x + K * v;
            matrix Pp_ = new matrix(P.rows, P.columns);

            Pp_ = P - K * H * P;
            for (int i = 1; i <= xp_.rows; i++)
            {
                for (int j = 1; j <= xp_.columns; j++)
                {
                    xp[i, j] = xp_[i, j];
                }
            }
            for (int i = 1; i <= x.rows; i++)
            {
                for (int j = 1; j <= x.rows; j++)
                {
                    Pp[i, j] = Pp_[i, j];
                }
            }
        }
예제 #7
0
        /*扩展卡尔曼滤波*/
        public static void kalman(double[] x, double[] R, double[] v, matrix P, matrix H, int nv)
        {
            matrix x_, xp_, P_, Pp_, H_, v_, R_;
            int    i, j, k;

            int[] ix = new int[nx];
            for (i = k = 0; i < nx; i++)
            {
                if (x[i] != 0 && P[i + 1, i + 1] > 0.0)
                {
                    ix[k++] = i;
                }
            }
            x_ = new matrix(k, 1); xp_ = new matrix(k, 1); P_ = new matrix(k, k); Pp_ = new matrix(k, k); H_ = new matrix(nv, k);
            v_ = new matrix(nv, 1); R_ = new matrix(nv, nv);
            for (i = 0; i < k; i++)
            {
                x_[i + 1, 1] = x[ix[i]];//待估参数
                for (j = 0; j < k; j++)
                {
                    P_[i + 1, j + 1] = P[ix[i] + 1, ix[j] + 1];//系统状态矩阵 k*k
                }
            }

            for (i = 0; i < nv; i++)//nv为观测方程个数,伪距和相位
            {
                for (j = 0; j < k; j++)
                {
                    H_[i + 1, j + 1] = H[i + 1, ix[j] + 1]; //观测矩阵 nv*k
                }
                v_[i + 1, 1]     = v[i];                    //伪距和相位观测残差 nv*1
                R_[i + 1, i + 1] = R[i];                    //观测噪声矩阵  nv*nv
            }
            filter(x_, P_, H_, R_, v_, xp_, Pp_);
            for (i = 0; i < k; i++)
            {
                x[ix[i]] = xp_[i + 1, 1];
                for (j = 0; j < k; j++)
                {
                    P[ix[i] + 1, ix[j] + 1] = Pp_[i + 1, j + 1];
                }
            }
        }
예제 #8
0
        /*精密单点定位*/
        public static int pppos(ppp_t pppt, List <obs_s> obs, nav_t nav, dcb_t dcb, station sta, erp_t erp)
        {
            int n = obs.Count, nv = 0, satprn;//当前历元观测的卫星个数

            double[][] rs  = new double[n][], dts = new double[n][], azel = new double[n][];
            double[]   var = new double[n], vare = new double[n];
            int[]      svh = new int[32];
            double[]   x   = new double[nx], v = new double[2 * n], R = new double[2 * n];
            matrix     P   = new matrix(37, 37);//
            matrix     H   = new matrix(2 * n, nx);

            for (int i = 0; i < n; i++)
            {
                rs[i]   = new double[6]; //卫星坐标和速度
                dts[i]  = new double[2]; //卫星的钟差和钟漂
                azel[i] = new double[2]; //卫星的方位角和高度角
            }

            //状态更新
            udstate(pppt, dcb, obs);
            pppcmn.satpos(obs, nav, sat, clk, pcv, rs, dts, vare, svh);

            for (int i = 0; i < nx; i++)
            {
                x[i] = pppt.x[i];
            }
            for (int i = 0; i < 10; i++)//滤波迭代
            {
                if ((nv = res_ppp(obs, sta, pppt, n, rs, dts, azel, vare, erp, dcb, x, R, v, H, svh)) <= 0)
                {
                    break;
                }

                for (int j = 0; j < nx; j++)
                {
                    for (int k = 0; k < nx; k++)
                    {
                        P[j + 1, k + 1] = pppt.P[j + 1, k + 1];
                    }
                }

                kalman(x, R, v, P, H, nv);
            }
            for (int i = 0; i < nx; i++)
            {
                pppt.x[i] = x[i];
                for (int j = 0; j < nx; j++)
                {
                    pppt.P[i + 1, j + 1] = P[i + 1, j + 1];
                }
            }
            for (int i = 0; i < 3; i++)
            {
                pppt.spp.rr[i] = pppt.x[i];
            }

            for (int i = 0; i < n; i++)
            {
                satprn = int.Parse(obs[i].sprn.Substring(1, 2));
                if (pppt.vsat[satprn - 1] == 0)
                {
                    continue;
                }
                pppt.outc[satprn - 1] = 0;
            }

            return(1);
        }
예제 #9
0
        /*矩阵求逆 LUP分解*/
        public static matrix inv(matrix a)
        {
            int n = a.rows;

            if (a.rows == a.columns)//是否为方阵
            {
                matrix b  = new matrix(n, n);
                matrix A  = matrix.matcpy(a);
                matrix L  = new matrix(n, n);
                matrix U  = new matrix(n, n);
                int[]  p  = new int[n];
                matrix L1 = matrix.eyes(n);
                matrix U1 = matrix.eyes(n);
                matrix P1 = new matrix(n, n);
                #region

                #endregion
                for (int i = 0; i < n; i++)
                {
                    p[i] = i + 1;//置换矩阵
                }
                for (int j = 1; j < n; j++)
                {
                    double tmpmax = 0;
                    int    imax = 0, itmp = 0;
                    for (int i = j; i <= n; i++)
                    {
                        if (Math.Abs(A[i, j]) > tmpmax)
                        {
                            tmpmax = A[i, j]; imax = i;
                        }
                    }
                    if (tmpmax == 0)
                    {
                        return(null);
                    }
                    if (imax != j)//交换
                    {
                        for (int k = 1; k <= n; k++)
                        {
                            tmpmax = A[j, k]; A[j, k] = A[imax, k]; A[imax, k] = tmpmax;
                        }
                        itmp = p[j - 1]; p[j - 1] = p[imax - 1]; p[imax - 1] = itmp;
                    }

                    for (int i = j + 1; i <= n; i++)//计算LU元素
                    {
                        A[i, j] = A[i, j] / A[j, j];
                        for (int k = j + 1; k <= n; k++)
                        {
                            A[i, k] = A[i, k] - A[i, j] * A[j, k];
                        }
                    }
                }
                //
                for (int i = 1; i <= n; i++)//分别得到LU
                {
                    L[i, i] = 1;
                    for (int j = 1; j <= n; j++)
                    {
                        if (i > j)
                        {
                            L[i, j] = A[i, j];
                        }
                        else
                        {
                            U[i, j] = A[i, j];
                        }
                    }
                    if (U[i, i] == 0)
                    {
                        return(null);
                    }
                }
                for (int i = 1; i <= n; i++)//U矩阵化为单位上三角矩阵
                {
                    double t2 = U[i, i];
                    for (int j = 1; j <= n; j++)
                    {
                        U[i, j]  = U[i, j] / t2;
                        U1[i, j] = U1[i, j] / t2;
                    }
                }
                for (int j = n; j > 1; j--)//U1,外循环从最后列开始
                {
                    for (int i = j - 1; i >= 1; i--)
                    {
                        for (int k = 1; k <= n; k++)
                        {
                            U1[i, k] = U1[i, k] - U[i, j] * U1[j, k];
                        }
                    }
                }
                for (int j = 1; j < n; j++)//L的逆,外循环从第一列开始
                {
                    for (int i = j + 1; i <= n; i++)
                    {
                        for (int k = 1; k <= n; k++)
                        {
                            L1[i, k] = L1[i, k] - L[i, j] * L1[j, k];
                        }
                    }
                }

                for (int i = 1; i <= n; i++)//P1
                {
                    P1[i, p[i - 1]] = 1;
                }
                b = U1 * L1 * P1;
                return(b);
            }
            else
            {
                return(null);
            }
        }