Пример #1
0
        public static void uptrop(ppp_t p3)
        {
            double[] pos = new double[3], azel = new double[2] {
                0.0, Math.PI / 2
            }, var = new double[1], rr = new double[3];
            double ztd;
            matrix pos_ = new matrix(3, 1);

            for (int i = 0; i < 3; i++)
            {
                rr[i] = p3.x[i];
            }
            if (p3.x[4] == 0)
            {
                transcoor.ecef2pos(matrix.Array2matrix(rr), pos_);
                for (int i = 0; i < 3; i++)
                {
                    pos[i] = pos_[i + 1, 1];
                }
                ztd = sbstrop(p3.spp.tcurr, pos, azel, var);
                initx(p3, ztd, var[0], 4);
            }
            else
            {
                p3.P[5, 5] += Math.Pow(1E-4, 2) * Math.Abs(p3.tt);
            }
        }
Пример #2
0
        public static void initx(ppp_t ppp, double xi, double var, int i)
        {
            int j;

            ppp.x[i] = xi;
            for (j = 0; j < nx; j++)
            {
                ppp.P[i + 1, j + 1] = ppp.P[j + 1, i + 1] = i == j ? var : 0;
            }
        }
Пример #3
0
        public static void udstate(ppp_t ppp, dcb_t dcb, List <obs_s> obs)
        {
            //更新位置
            if ((ppp.x[0] * ppp.x[0] + ppp.x[1] * ppp.x[1] + ppp.x[2] * ppp.x[2]) <= 0)//第一个历元
            {
                for (int i = 0; i < 3; i++)
                {
                    initx(ppp, ppp.spp.rr[i], VAR_POS, i);
                }
            }
            //更新钟差
            initx(ppp, ppp.spp.dtr * CLIGHT, VAR_CLK, 3);

            //更新对流层参数
            uptrop(ppp);
            //更新相位偏差
            upbias_ppp(ppp, obs, dcb);
        }
Пример #4
0
        public static void propos(obs_t obss, dcb_t dcb, pcv_t pcv, station sta, nav_t nav, erp_t erp, List <result> res, RichTextBox text)
        {
            int     nobs    = 0;
            rtktime solt    = new rtktime();
            string  timestr = null;
            obs_t   obs     = new obs_t();
            ppp_t   p3      = new ppp_t();
            spp_t   spp     = new spp_t();

            while ((nobs = inputobs(obs, obss)) > 0)
            {
                result re = new result();
                solt = spp.tcur;
                if (pntpos(obs.obs_b, nav, spp, dcb) == 0)
                {
                    continue;
                }
                p3.spp     = spp;
                p3.soltime = spp.tcur;
                if (solt.time_int != 0)
                {
                    p3.tt = rtklibcmn.timediff(p3.soltime, solt);
                }
                if (pppos(p3, obs.obs_b, nav, dcb, sta, erp) != 1)
                {
                    continue;
                }

                for (int j = 0; j < 3; j++)
                {
                    spp.rr[j] = p3.x[j];
                }
                spp.dtr = p3.x[4];
                re.time = obs.obs_b[0].t.calend;
                re.X    = p3.x[0]; re.Y = p3.x[1]; re.Z = p3.x[2];
                timestr = re.time[0] + "/" + re.time[1] + "/" + re.time[2] + " " + re.time[3] + ":" + re.time[4] + ":" + re.time[5];
                text.AppendText(string.Format("{0,-20}", timestr) + string.Format("{0,-20}", re.X) + string.Format("{0,-20}", re.Y) + string.Format("{0,-20}", re.Z) + "\r\n");

                res.Add(re);
            }
        }
Пример #5
0
        /*GF组合探测周跳*/
        public static void detslp_gf(ppp_t p3, List <obs_s> obs, int n)
        {
            double g0, g1;
            int    i, sat;

            for (i = 0; i < n; i++)
            {
                sat = int.Parse(obs[i].sprn.Substring(1, 2));
                if ((g1 = gfmeas(obs[i])) == 0)
                {
                    continue;
                }
                g0             = p3.gf[sat - 1];
                p3.gf[sat - 1] = g1;

                if (g0 != 0 && Math.Abs(g1 - g0) > 0.5)
                {
                    p3.cyslip[sat - 1] = 1;
                }
            }
        }
Пример #6
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);
        }
Пример #7
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);
            }
        }
Пример #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);
        }