Exemplo n.º 1
0
        /// <summary>
        /// 如果有效,则直接返回,如果无效则重置先验值重新计算。
        /// </summary>
        /// <param name="epochInfo"></param>
        /// <param name="result"></param>
        /// <returns></returns>
        private SingleSiteGnssResult CheckOrRecaculate(EpochInformation epochInfo, SingleSiteGnssResult result)
        {
            if (Geo.Algorithm.Vector.IsValid(result.ResultMatrix.StdOfEstimatedParam))
            {
                return(result);
            }

            var lastOkIndex = PrevResults.LastKey;
            var differIndex = CurrentIndex - lastOkIndex;

            var newAprioriIndex = PrevResults.LastKey - differIndex;

            while (newAprioriIndex > 0)
            {
                if (PrevResults.Contains(newAprioriIndex))
                {
                    var product = PrevResults[newAprioriIndex];
                    this.SetProduct(product);

                    if (differIndex > 5)
                    {
                        //this.MatrixBuilder.IsResetCovaOfApriori = true;
                    }

                    break;
                }
                newAprioriIndex--;
            }

            return(base.CaculateKalmanFilter(epochInfo, this.CurrentProduct));
        }
Exemplo n.º 2
0
        /// <summary>
        /// 单点定位结果转化为Sinex文件
        /// </summary>
        /// <returns></returns>
        public static SinexFile Build(SingleSiteGnssResult result)
        {
            SinexFile sinex = new SinexFile("Gnsser");

            sinex.SolutionEpochBlock.Items.Add(new SolutionEpoch()
            {
                DateStart       = result.ReceiverTime,
                DateEnd         = result.ReceiverTime,
                DateMean        = result.ReceiverTime,
                ObservationCode = "P",
                PointCode       = "A",
                SiteCode        = result.SiteInfo.SiteName,
                SolutionID      = "0001"
            });
            SinexStatistic stat = new SinexStatistic()
            {
                NumberOfUnknown          = result.ResultMatrix.ParamCount,
                NumberOfObservations     = result.ResultMatrix.ObsMatrix.Observation.Count,
                VarianceOfUnitWeight     = result.ResultMatrix.VarianceOfUnitWeight,
                NumberOfDegreesOfFreedom = result.ResultMatrix.Freedom
            };

            sinex.SolutionStattisticsBlock.Items = stat.GetSolutionStatistics();

            GeoCoord approxGeo = CoordTransformer.XyzToGeoCoord(result.ApproxXyz);

            sinex.SiteIdBlock.Items.Add(new SiteId()
            {
                SiteCode             = result.SiteInfo.SiteName,
                PointCode            = "A",
                UniqueMonumentId     = result.SiteInfo.MarkerNumber,
                ApproximateHeight    = approxGeo.Height,
                ApproximateLatitude  = approxGeo.Lat,
                ApproximateLongitude = approxGeo.Lon,
                ObservationCode      = "P",
                GeoCoord             = result.GeoCoord,
                StationDescription   = "Single Point"
            });
            int index = 0;

            sinex.SolutionEstimateBlock.Items.AddRange(new SolutionValue[] {
                new SolutionValue()
                {
                    Index          = 1 + index++,
                    ParameterType  = "STAX",
                    ParameterValue = result.EstimatedXyz.X,
                    SiteCode       = result.SiteInfo.SiteName,
                    RefEpoch       = result.ReceiverTime,
                    PointCode      = "A",
                    ParameterUnits = "m",
                    ConstraintCode = "2",
                    StdDev         = Math.Sqrt(result.ResultMatrix.CovaOfEstimatedParam[index - 1, index - 1]),
                    SolutionID     = "0001"
                }, new SolutionValue()
                {
                    Index          = 1 + index++,
                    ParameterType  = "STAY",
                    ParameterValue = result.EstimatedXyz.Y,
                    SiteCode       = result.SiteInfo.SiteName,
                    RefEpoch       = result.ReceiverTime,
                    PointCode      = "A",
                    ParameterUnits = "m",
                    ConstraintCode = "2",
                    StdDev         = Math.Sqrt(result.ResultMatrix.CovaOfEstimatedParam[index - 1, index - 1]),
                    SolutionID     = "0001"
                }, new SolutionValue()
                {
                    Index          = 1 + index++,
                    ParameterType  = "STAZ",
                    ParameterValue = result.EstimatedXyz.Z,
                    SiteCode       = result.SiteInfo.SiteName,
                    RefEpoch       = result.ReceiverTime,
                    PointCode      = "A",
                    ParameterUnits = "m",
                    ConstraintCode = "2",
                    StdDev         = Math.Sqrt(result.ResultMatrix.CovaOfEstimatedParam[index - 1, index - 1]),
                    SolutionID     = "0001"
                },
                new SolutionValue()
                {
                    Index          = 1 + index++,
                    ParameterType  = "RBIAS",
                    ParameterValue = result.RcvClkErrDistance,
                    SiteCode       = result.SiteInfo.SiteName,
                    RefEpoch       = result.ReceiverTime,
                    PointCode      = "A",
                    ParameterUnits = "m",
                    ConstraintCode = "2",
                    StdDev         = Math.Sqrt(result.ResultMatrix.CovaOfEstimatedParam[index - 1, index - 1]),
                    SolutionID     = "0001"
                }
            });

            //  sinex.
            sinex.SolutionMatrixEstimateCova.Items = SinexMatrixConvertor.GetMatrixLines(result.ResultMatrix.CovaOfEstimatedParam.Array);

            return(sinex);
        }
Exemplo n.º 3
0
        /// <summary>
        /// PPP 计算核心方法。 Kalmam滤波。
        /// 观测量的顺序是先伪距观测量,后载波观测量,观测量的总数为卫星数量的两倍。
        /// 参数数量为卫星数量加5,卫星数量对应各个模糊度,5为3个位置量xyz,1个接收机钟差量,1个对流程湿分量。
        /// </summary>
        /// <param name="recInfo">接收信息</param>
        /// <param name="option">解算选项</param>
        /// <param name="lastPppResult">上次解算结果(用于 Kalman 滤波),若为null则使用初始值计算</param>
        /// <returns></returns>
        public override SingleSiteGnssResult CaculateKalmanFilter(EpochInformation recInfo, SingleSiteGnssResult lastPppResult = null)
        {
            PppResult last = null;

            if (lastPppResult != null)
            {
                last = (PppResult)lastPppResult;
            }
            //  ISatWeightProvider SatWeightProvider = new SatElevateAndRangeWeightProvider();
            //ISatWeightProvider SatWeightProvider = new SatElevateWeightProvider();


            MatrixBuilder.SetMaterial(recInfo).SetPreviousProduct(lastPppResult);
            MatrixBuilder.Build();

            if (this.MatrixBuilder.ObsCount > 0)
            {
                //  this.Adjustment = new KalmanFilter( this.MatrixBuilder);
                this.Adjustment = this.RunAdjuster(BuildAdjustObsMatrix(this.CurrentMaterial));


                ////尝试固定模糊度  cuiyang 2015.07
                //int fixFlag = Ppp_AR.Process(recInfo, Adjustment);

                #region 具有条件的参数平差
                // this.Adjustment.restrictadjust();
                #endregion

                #region PPP模糊度固定
                foreach (var sat in recInfo.EnabledPrns)
                {
                    if (recInfo.SlipedPrns.Contains(sat))    //主要是针对 1.首次观测到某颗卫星 2.某颗卫星出现在第二个弧段,两个弧段的MW不相等
                    {
                        if (MwInfoDic.ContainsKey(sat))
                        {
                            MwInfoDic.Remove(sat);
                        }
                        continue;
                    }

                    if (!MwInfoDic.ContainsKey(sat))        //第一个没有周跳的历元
                    {
                        MwInfoDic[sat] = new SmoothValue(100, true)
                        {
                            Name = sat.ToString()
                        };
                    }

                    MwInfoDic[sat].Regist(recInfo[sat].Combinations.MwRangeCombination.Value);
                }

                List <int> cyclesat = new List <int>();   //将有周跳的卫星挑出来
                foreach (var sat in recInfo.SlipedPrns)
                {
                    cyclesat.Add(sat.PRN);
                }
                if (MwInfoDic.Count > 0)
                {
                    Dictionary <int, double> MWs = new Dictionary <int, double>();
                    foreach (var sat in recInfo.EnabledPrns)
                    {
                        if (!recInfo.SlipedPrns.Contains(sat))
                        {
                            int num = sat.PRN;
                            MWs.Add(num, MwInfoDic[sat].Value);    //宽巷模糊度的历元平滑值
                        }
                    }
                    IMatrix new_estimated = this.Adjustment.PppArmbiResolve(MWs, cyclesat);
                }
                #endregion



                PppResult result = new PppResult(recInfo, Adjustment, this.MatrixBuilder.GnssParamNameBuilder);
                //  result.PreviousResult = lastPppResult;

                //模糊度设置
                IonoFreeAmbiguityMgr.SetIonoFreeCombination(result);


                return(result);
            }
            else
            {
                return(null);
            }
        }
Exemplo n.º 4
0
        /// <summary>
        /// 快速计算一个坐标。自动重置数据流。
        /// </summary>
        /// <param name="DataSourceContext"></param>
        /// <returns></returns>
        public static SingleSiteGnssResult GetApproxPosition(DataSourceContext DataSourceContext)
        {
            GnssProcessOption PositionOption = GnssProcessOption.GetDefaultSimplePseudoRangePositioningOption();
            var p = new SimpleRangePositioner(DataSourceContext, PositionOption)
            {
                IsUpdateEstimatePostition = true,
                IsPostCheckEnabled        = false,//取消检查,否则报错,2019.05.16, czs
            };

            var Reviser = EpochInfoReviseManager.GetDefaultEpochInfoReviser(DataSourceContext, PositionOption, null);
            var checker = EpochCheckingManager.GetDefaultCheckers(DataSourceContext, PositionOption);
            var EpochEphemerisSetter = new EpochEphemerisSetter(DataSourceContext, PositionOption);

            lock (locker)
            {
                var initMaxRms      = PositionOption.MaxStdDev;
                var initType        = PositionOption.CaculateType;
                var initUpdate      = PositionOption.IsUpdateEstimatePostition;
                var initPostCheck   = PositionOption.IsResidualCheckEnabled;
                var initResultCheck = PositionOption.IsResultCheckEnabled;

                PositionOption.CaculateType              = CaculateType.Independent;
                PositionOption.MaxStdDev                 = 9999999;
                PositionOption.IsResidualCheckEnabled    = false;
                PositionOption.IsResultCheckEnabled      = false;
                PositionOption.IsUpdateEstimatePostition = initUpdate;

                int i = 0;
                SingleSiteGnssResult        GnssResult = null;
                List <SingleSiteGnssResult> results    = new List <SingleSiteGnssResult>();
                foreach (var item in DataSourceContext.ObservationDataSource)
                {
                    i++;

                    if (!checker.Check(item))
                    {
                        continue;
                    }
                    var info = item;
                    // if (!Reviser.Revise(ref info)) { continue; }
                    Reviser.Revise(ref info);
                    if (info.EnabledSatCount < 4)
                    {
                        continue;
                    }
                    GnssResult = p.Get(item);
                    if (GnssResult != null)
                    {
                        item.SiteInfo.SetApproxXyz(item.SiteInfo.EstimatedXyz);
                        results.Add(GnssResult);
                    }
                    if (i > 5 && results.Count > 0)
                    {
                        results.Sort(new Comparison <SingleSiteGnssResult>(delegate(SingleSiteGnssResult a, SingleSiteGnssResult b) { return((int)(1000 * (a.EstimatedXyzRms.Length - b.EstimatedXyzRms.Length))); }));
                        GnssResult = results[0];
                        break;
                    }
                }
                PositionOption.IsResidualCheckEnabled    = initPostCheck;
                PositionOption.IsUpdateEstimatePostition = initUpdate;
                PositionOption.IsResultCheckEnabled      = initResultCheck;
                PositionOption.CaculateType = initType;
                DataSourceContext.ObservationDataSource.Reset();
                PositionOption.MaxStdDev = initMaxRms;
                return(GnssResult);
            }
        }
Exemplo n.º 5
0
        /// <summary>
        /// 滤波计算
        /// </summary>
        /// <param name="epochInfo"></param>
        /// <param name="lastResult"></param>
        /// <returns></returns>
        public override SingleSiteGnssResult CaculateKalmanFilter(EpochInformation epochInfo, SingleSiteGnssResult lastResult)
        {
            //极速模式。
            epochInfo.RemoveIonoFreeUnavailable();
            if (epochInfo.Count < 2)
            {
                log.Error("卫星可用数量不足:" + epochInfo.Count);
                return(null);
            }
            var result = base.CaculateKalmanFilter(epochInfo, lastResult) as PppResult;

            //外部模糊度文件直接固定
            if (Option.IsFixingAmbiguity && Option.IsUseFixedParamDirectly && File.Exists(Option.AmbiguityFilePath) && Option.IsUsingAmbiguityFile)
            {
                return(result);
            }


            return(result);
        }
Exemplo n.º 6
0
        public override SingleSiteGnssResult CaculateKalmanFilter(EpochInformation epochInfo, SingleSiteGnssResult lastResult)
        {
            CurrentIndex++;

            var result = base.CaculateKalmanFilter(epochInfo, lastResult);

            if (CurrentProduct != null)
            {
                if (XYZ.IsValueValid(CurrentProduct.EstimatedXyzRms))
                {
                    LastValidXyzRms = CurrentProduct.EstimatedXyzRms;
                }
            }

            result = CheckOrRecaculate(epochInfo, result);

            if (!XYZ.IsValueValid(result.EstimatedXyzRms))
            {
                log.Error("RMS 检核未通过 " + result.EstimatedXyzRms);
                return(null);
            }


            //if (LastValidXyzRms != null
            //    && CurrentIndex > 10 //先让其浪一会儿
            //    && (result.EstimatedXyzRms.Length > LastValidXyzRms.Length * 50))
            //{
            //    log.Error("RMS 检核未通过 " + result.EstimatedXyzRms);
            //    return null;
            //}
            //ok
            PrevResults.Add(CurrentIndex, result);

            return(result);
        }
Exemplo n.º 7
0
        /// <summary>
        /// 滤波计算
        /// </summary>
        /// <param name="epochInfo"></param>
        /// <param name="lastResult"></param>
        /// <returns></returns>
        public override SingleSiteGnssResult CaculateKalmanFilter(EpochInformation epochInfo, SingleSiteGnssResult lastResult)
        {
            if (RangePositioner != null)
            {
                var posResult = RangePositioner.CurrentProduct;
            }
            var result = base.CaculateKalmanFilter(epochInfo, lastResult) as PppResult;

            #region
            if (this.IsFixingAmbiguity && epochInfo.Time.Value.SecondsOfDay > epochInfo.ObsInfo.StartTime.SecondsOfDay + 120)//&& this.Adjustment.ParamCount>10000000
            {
                #region 读取自定义FCB文件
                string WLpath = Path.Combine(Setting.DataDirectory, @"GNSS\Attachment\2013001FCB\WLFCB.txt");
                Dictionary <SatelliteNumber, double> SDWL_FCB = new Dictionary <SatelliteNumber, double>();
                Dictionary <SatelliteNumber, double> SDNL_FCB = new Dictionary <SatelliteNumber, double>();
                using (var streamReader = new StreamReader(WLpath))
                {
                    var  line = "";
                    Time time = Time.Default;
                    while ((line = streamReader.ReadLine()) != null)
                    {
                        SatelliteNumber prn = SatelliteNumber.Parse(line.Substring(0, 3));
                        var             val = Double.Parse(line.Substring(4));
                        SDWL_FCB.Add(prn, val);
                    }
                }

                //???NLFCB.txt 文件头注释采用什么星历和钟差估计的FCB
                string NLpath = Path.Combine(Setting.DataDirectory, @"GNSS\Attachment\2013001FCB\NLFCB.txt");
                using (var streamReader = new StreamReader(NLpath))
                {
                    var  line = "";
                    Time time = Time.Default;
                    while ((line = streamReader.ReadLine()) != null)
                    {
                        SatelliteNumber prn = SatelliteNumber.Parse(line.Substring(0, 3));
                        var             val = Double.Parse(line.Substring(4));
                        SDNL_FCB.Add(prn, val);
                    }
                }
                #endregion

                //???此处采用G01,应改为通用
                SatelliteNumber MaxPrn = SatelliteNumber.Parse("G01");

                #region MW值逐历元平滑
                List <SatelliteNumber> SatWithCycleSlips = new List <SatelliteNumber>();
                //epochInfo.GetCycleSlipedPrns();
                Dictionary <SatelliteNumber, double> Smooth_MWs = new Dictionary <SatelliteNumber, double>();
                foreach (var sat in epochInfo.TotalPrns)
                {
                    if (SatWithCycleSlips.Contains(sat))//主要是针对 1.首次观测到某颗卫星 2.某颗卫星出现在第二个弧段,两个弧段的MW不相等&& epochInfo.Time.Value.Minute== 21&& epochInfo.Time.Value.Second ==30
                    {
                        if (SUM_MWs.ContainsKey(sat))
                        {
                            SUM_MWs.Remove(sat);
                        }
                        continue;
                    }

                    if (SUM_MWs.ContainsKey(sat))//累加
                    {
                        //SUM_MWs[sat].sum += epochInfo.MWs[sat];
                        SUM_MWs[sat].sum   += epochInfo[sat].MwCycle;
                        SUM_MWs[sat].total += 1;
                    }
                    else //第一个没有周跳的历元
                    {
                        MWInfo tmpmw = new MWInfo();
                        //tmpmw.sum = epochInfo.MWs[sat];
                        tmpmw.sum   = epochInfo[sat].MwCycle;
                        tmpmw.total = 1;
                        SUM_MWs.Add(sat, tmpmw);
                    }
                }

                //当前历元没有出现改课卫星时,要将其从SUM_MWs清除
                List <SatelliteNumber> tmpsats = new List <SatelliteNumber>(SUM_MWs.Keys);
                foreach (var sat in tmpsats)
                {
                    if (epochInfo.EnabledPrns.Contains(sat))
                    {
                        continue;
                    }
                    else
                    {
                        SUM_MWs.Remove(sat);
                    }
                }

                if (SUM_MWs.Count > 0)
                {
                    foreach (var sat in epochInfo.TotalPrns)
                    {
                        if (!SatWithCycleSlips.Contains(sat))
                        {
                            Smooth_MWs.Add(sat, SUM_MWs[sat].sum / SUM_MWs[sat].total);//宽巷模糊度的历元平滑值
                        }
                    }
                }
                #endregion
                #region 读取FCB文件
                //string path = "D:\\Data\\GNSS\\19240FCB\\sgg19240_igs_30.fcb";
                //FcbFileReader reader = new FcbFileReader(path);
                //FcbFile = reader.Read();

                ////对其到15min的FCB
                //double  tmp  = Math.Floor(epochInfo.Time.Value.SecondsOfDay / 900) * 900 + Math.Floor(epochInfo.Time.Value.SecondsOfWeek / 86400) *86400;
                //Time current = new Time(epochInfo.Time.Value.GpsWeek, tmp);

                //var data = FcbFile.FcbInfos;
                //var currentdata = data.FindAll(m => m.Time == current);
                #endregion
                #region 所有卫星的星间单差宽巷及窄巷FCB
                //选择高度角最大的那颗卫星

                //SatelliteNumber MaxPrn;
                //MaxPrn = epochInfo[0].Prn;
                //foreach (var item in epochInfo)
                //{
                //    if (epochInfo[MaxPrn].Polar.Elevation < item.Polar.Elevation)
                //    {
                //        MaxPrn = item.Prn;
                //    }
                //    else
                //    {
                //        continue;
                //    }
                //}
                ////及那颗卫星的FCB
                //double ReferenceNLFCB = 0.0;
                //foreach(var item in currentdata)
                //{
                //    if(item.Prn == MaxPrn)
                //    {
                //        ReferenceNLFCB = item.Value;
                //    }
                //}
                ////星间单差FCB
                //Dictionary<SatelliteNumber, double> SDNL_FCB = new Dictionary<SatelliteNumber, double>();
                //foreach(var item in currentdata)
                //{
                //    SDNL_FCB.Add(item.Prn, item.Value - ReferenceNLFCB);
                //}

                //var wl = FcbFile.Header.WideLaneValue;
                //double ReferenceWLFCB = 0.0;
                //foreach (var item in wl.Data)
                //{
                //    if (item.Key == MaxPrn)
                //    {
                //        ReferenceWLFCB = item.Value.Value;
                //    }
                //}
                ////星间单差宽巷FCB
                //Dictionary<SatelliteNumber, double> SDWL_FCB = new Dictionary<SatelliteNumber, double>();
                //foreach (var item in wl.Data)
                //{
                //    SDWL_FCB.Add(item.Key, item.Value.Value - ReferenceWLFCB);
                //}
                #endregion


                #region 星间单差宽巷模糊度//星间单差宽巷模糊度,减去参考星,不包括出现周跳的卫星
                Dictionary <SatelliteNumber, double> SD_MW = new Dictionary <SatelliteNumber, double>();
                foreach (var item in Smooth_MWs)
                {
                    if (item.Key == MaxPrn)
                    {
                        continue;
                    }
                    SD_MW.Add(item.Key, item.Value - Smooth_MWs[MaxPrn]);
                }
                #endregion

                #region 对MW值(宽巷模糊度)进行宽巷FCB纠正,注意是-,是减
                Dictionary <SatelliteNumber, double> correctedSD_MW = new Dictionary <SatelliteNumber, double>();
                foreach (var item in SD_MW)
                {
                    correctedSD_MW.Add(item.Key, item.Value - SDWL_FCB[item.Key]); //改正WLFCB
                }
                Dictionary <SatelliteNumber, double> fixedSD_MW = new Dictionary <SatelliteNumber, double>();
                foreach (var item in correctedSD_MW)
                {
                    if ((Math.Abs(correctedSD_MW[item.Key] - Math.Round(correctedSD_MW[item.Key])) <= 0.25))  //NLFCBs=0代表无效
                    {
                        fixedSD_MW.Add(item.Key, Math.Round(correctedSD_MW[item.Key]));
                    }
                }
                #endregion

                #region 对窄巷模糊度进行NLFCB改正,注意是-
                Dictionary <SatelliteNumber, double> floatAmb = new Dictionary <SatelliteNumber, double>();//存储无电离层浮点模糊度,有参考星
                for (int i = 5; i < this.Adjustment.ParamCount; i++)
                {
                    SatelliteNumber sat = SatelliteNumber.Parse(this.Adjustment.ParamNames[i].Substring(0, 3));
                    //if (SatWithCycleSlips.Contains(sat)) continue;
                    floatAmb.Add(sat, this.Adjustment.Estimated[i, 0]);                                        //估值
                }
                Dictionary <SatelliteNumber, double> SD_floatAmb = new Dictionary <SatelliteNumber, double>(); //存储星间单差无电离层浮点模糊度,删除参考星,单位:周
                foreach (var sat in floatAmb.Keys)
                {
                    if (sat == MaxPrn)
                    {
                        continue;
                    }
                    SD_floatAmb.Add(sat, (floatAmb[sat] - floatAmb[MaxPrn]) / GnssConst.GPS_L1_WAVELENGTH);//
                }

                Dictionary <SatelliteNumber, double> SDNL_floatAmb = new Dictionary <SatelliteNumber, double>();//存储窄巷浮点模糊度,没有参考星,注意是- ??????????
                foreach (var sat in fixedSD_MW.Keys)
                {
                    if (SDNL_FCB[sat] != 0)
                    {
                        double tmpnl = SD_floatAmb[sat] * (1575.42 + 1227.60) / 1575.42 - fixedSD_MW[sat] * 1227.60 / (1575.42 - 1227.60) - SDNL_FCB[sat];
                        SDNL_floatAmb.Add(sat, tmpnl);
                    }
                }
                #endregion

                #region LAMBDA固定窄巷模糊度
                if (SDNL_floatAmb.Count >= 4) //模糊度维度>4的情况才进行固定
                {
                    #region 星间单差窄巷模糊度协方差矩阵
                    IMatrix B     = new ArrayMatrix(this.Adjustment.ParamNames.Count - 6, this.Adjustment.ParamNames.Count - 5, 0.0); //将非差模糊度转换为星间单差模糊度的旋转矩阵
                    int     index = 0;                                                                                                //参考星的索引位置???,参考星不同,index不同
                    for (int i = 5; i < this.Adjustment.ParamNames.Count; i++)
                    {
                        SatelliteNumber sat = SatelliteNumber.Parse(this.Adjustment.ParamNames[i].Substring(0, 3));
                        if (sat == MaxPrn)
                        {
                            break;
                        }
                        else
                        {
                            index++;
                        }
                    }
                    for (int i = 0; i < this.Adjustment.ParamNames.Count - 6; i++)
                    {
                        if (i < index)
                        {
                            B[i, index] = -1; B[i, i] = 1;
                        }
                        if (i >= index)
                        {
                            B[i, index] = -1; B[i, i + 1] = 1;
                        }
                    }

                    IMatrix BT      = B.Transposition;
                    IMatrix covaAmb = new ArrayMatrix(this.Adjustment.ParamNames.Count - 5, this.Adjustment.ParamNames.Count - 5, 0); //非差模糊度的协方差矩阵
                    for (int i = 0; i < this.Adjustment.ParamNames.Count - 5; i++)
                    {
                        for (int j = 0; j < this.Adjustment.ParamNames.Count - 5; j++)
                        {
                            covaAmb[i, j] = this.Adjustment.CovaOfEstimatedParam[i + 5, j + 5];
                        }
                    }
                    IMatrix SD_covaAmb   = B.Multiply(covaAmb).Multiply(BT);                                                                 //星间单差模糊度的协方差矩阵
                    IMatrix SDNL_covaAmb = new ArrayMatrix(this.Adjustment.ParamNames.Count - 6, this.Adjustment.ParamNames.Count - 6, 0.0); //星间单差窄巷模糊度的协方差矩阵
                    for (int i = 0; i < this.Adjustment.ParamNames.Count - 6; i++)
                    {
                        for (int j = 0; j < this.Adjustment.ParamNames.Count - 6; j++)
                        {
                            SDNL_covaAmb[i, j] = SD_covaAmb[i, j] * (1575.42 + 1227.60) / (1575.42 * GnssConst.GPS_L1_WAVELENGTH);//系数
                        }
                    }
                    #endregion

                    #region 部分模糊度固定,先将可以固定的星间单差窄巷模糊度 及其 协方差矩阵挑出来
                    List <string> old_ambpara = new List <string>();
                    foreach (var item in SD_floatAmb.Keys)
                    {
                        old_ambpara.Add(item.ToString());//所有的模糊度(除了参考星)
                    }
                    List <string> new_ambpara = new List <string>();
                    foreach (var item in SDNL_floatAmb.Keys)
                    {
                        new_ambpara.Add(item.ToString());                                                     //可以固定的模糊度(宽巷 < 0.25 && NLFCB != 0)
                    }
                    IMatrix newtrix = NamedMatrix.GetNewMatrix(new_ambpara, old_ambpara, SDNL_covaAmb.Array); //将可以固定的模糊度的协方差矩阵挑出来
                    #endregion

                    #region LAMBDA星间单差窄巷模糊度固定
                    IMatrix narrowLaneAmbiguity = new ArrayMatrix(SDNL_floatAmb.Count, 1, 0);
                    int     ii = 0;
                    foreach (var item in SDNL_floatAmb.Values)
                    {
                        narrowLaneAmbiguity[ii, 0] = item;
                        ii++;
                    }
                    #region 降维,但是存在相等就麻烦了
                    double[]   a      = narrowLaneAmbiguity.GetCol(0).OneDimArray;
                    double[][] Q      = newtrix.Array;
                    LlyLambda  lambda = new LlyLambda(new_ambpara.Count, 2, Q, a);//LAMBDA算法
                    double[]   N1     = new double[a.Length * 2]; for (int i = 0; i < a.Length; i++)
                    {
                        N1[i] = Math.Floor(a[i] + 0.5);
                    }
                    double[] s    = new double[2];
                    int      info = 0;
                    info = lambda.getLambda(ref N1, ref s);
                    double ratio = s[1] / s[0];
                    #endregion
                    #endregion

                    //宽巷模糊度和窄巷模糊度固定后反求出的无电离层组合模糊度
                    Dictionary <SatelliteNumber, double> fixedIonoAmb = new Dictionary <SatelliteNumber, double>();
                    #region 求固定后的无电离层模糊度

                    if (ratio > 3)
                    {
                        if (N1.Length == newtrix.RowCount * 2)//没有出现降维
                        {
                            List <SatelliteNumber> validsats = new List <SatelliteNumber>(SDNL_floatAmb.Keys);
                            for (int i = 0; i < newtrix.RowCount; i++)
                            {
                                SatelliteNumber sat       = validsats[i];
                                double          currentWL = fixedSD_MW[sat]; //该卫星的宽巷,注意是整数
                                double          currentNL = N1[i];           //该卫星的窄巷,注意是整数
                                // double freqOfN =

                                double freqOfW        = 1575.42 * 1227.60 / (1575.42 * 1575.42 - 1227.6 * 1227.60);
                                double freqOfN        = 1575.42 / (1575.42 + 1227.60);
                                double currentIonoAmb = freqOfW * currentWL + freqOfN * currentNL + freqOfN * SDNL_FCB[sat];
                                fixedIonoAmb.Add(sat, currentIonoAmb);
                            }
                        }
                        IMatrix restrictMatrix = new ArrayMatrix(fixedIonoAmb.Count, this.Adjustment.ParamCount, 0); //约束矩阵
                        IMatrix W = new ArrayMatrix(fixedIonoAmb.Count, 1, 0);                                       //W矩阵
                        ii = 0;
                        List <SatelliteNumber> totalsats = new List <SatelliteNumber>(epochInfo.EnabledPrns);
                        foreach (var sat in fixedIonoAmb.Keys)
                        {
                            int currentIndex = totalsats.IndexOf(sat);//获取索引值
                            restrictMatrix[ii, 5 + index]        = -1;
                            restrictMatrix[ii, 5 + currentIndex] = 1;
                            W[ii, 0] = (SD_floatAmb[sat] - fixedIonoAmb[sat]) * GnssConst.GPS_L1_WAVELENGTH;
                            ii++;
                        }
                        #region 具有约束条件的参数平差 ???条件平差给不同的权,得到不同的结果
                        IMatrix A        = restrictMatrix;
                        IMatrix AT       = A.Transposition;
                        IMatrix AQAT     = A.Multiply(this.Adjustment.CovaOfEstimatedParam).Multiply(AT);
                        IMatrix Inv_AQAT = AQAT.GetInverse();
                        IMatrix tmp1     = this.Adjustment.CovaOfEstimatedParam.Multiply(AT).Multiply(Inv_AQAT);
                        IMatrix tmp2     = tmp1.Multiply(W);
                        IMatrix X1       = new ArrayMatrix(this.Adjustment.ParamNames.Count, 1, 0);
                        for (int i = 0; i < this.Adjustment.ParamNames.Count; i++)
                        {
                            X1[i, 0] = this.Adjustment.Estimated[i, 0];
                        }
                        IMatrix new1_Estimated = X1.Minus(tmp2);//估值

                        IMatrix tmp3 = tmp1.Multiply(A).Multiply(this.Adjustment.CovaOfEstimatedParam);
                        IMatrix new_CovaOfEstimatedParam = this.Adjustment.CovaOfEstimatedParam.Minus(tmp3);//协方差矩阵
                        IMatrix P2 = new ArrayMatrix(fixedIonoAmb.Count, fixedIonoAmb.Count, 0);
                        for (int kk = 0; kk < fixedIonoAmb.Count; kk++)
                        {
                            P2[kk, kk] = 5 * Math.Pow(10, 6); //给FCB的一个权值,暂定5*10^6
                        }
                        IMatrix m1                        = P2.GetInverse();
                        IMatrix m2                        = m1.Plus(AQAT);
                        IMatrix m3                        = m2.GetInverse();
                        IMatrix J                         = this.Adjustment.CovaOfEstimatedParam.Multiply(AT).Multiply(m3);
                        IMatrix M4                        = J.Multiply(W);
                        IMatrix new2_Estimated            = this.Adjustment.Estimated;
                        IMatrix new2_CovaOfEstimatedParam = this.Adjustment.CovaOfEstimatedParam;
                        if (Math.Sqrt(M4[0, 0] * M4[0, 0] + M4[1, 0] * M4[1, 0] + M4[2, 0] * M4[2, 0]) < 0.001)
                        {
                            new2_Estimated            = this.Adjustment.Estimated;
                            new2_CovaOfEstimatedParam = this.Adjustment.CovaOfEstimatedParam;
                        }
                        else
                        {
                            new2_Estimated = X1.Minus(M4);
                            IMatrix m5 = J.Multiply(A).Multiply(this.Adjustment.CovaOfEstimatedParam);
                            new2_CovaOfEstimatedParam = this.Adjustment.CovaOfEstimatedParam.Minus(m5);
                        }
                        this.Adjustment.Estimated_PPPAR1 = new WeightedVector(new1_Estimated, new_CovaOfEstimatedParam);
                        this.Adjustment.Estimated_PPPAR2 = new WeightedVector(new2_Estimated, new2_CovaOfEstimatedParam);
                        #endregion
                    }
                    #endregion
                }
                #endregion
            }
            #endregion
            return(result);
        }
Exemplo n.º 8
0
 /// <summary>
 ///  迭代求解
 /// </summary>
 /// <param name="epochInfo"></param>
 /// <param name="lastResult"></param>
 /// <returns></returns>
 public override SingleSiteGnssResult CaculateKalmanFilter(
     EpochInformation epochInfo,
     SingleSiteGnssResult lastResult = null)
 {
     return(Caculate(epochInfo));
 }
Exemplo n.º 9
0
        public override SingleSiteGnssResult CaculateKalmanFilter(EpochInformation epochInfo, SingleSiteGnssResult lastResult)
        {
            //    return Caculate(epochInfo);

            return(base.CaculateKalmanFilter(epochInfo, lastResult));
        }
Exemplo n.º 10
0
        /// <summary>
        /// 滤波计算
        /// </summary>
        /// <param name="epochInfo"></param>
        /// <param name="lastResult"></param>
        /// <returns></returns>
        public override SingleSiteGnssResult CaculateKalmanFilter(EpochInformation epochInfo, SingleSiteGnssResult lastResult)
        {
            var result = base.CaculateKalmanFilter(epochInfo, lastResult) as RecursiveIonoFreePppResult;

            return(result);
        }
Exemplo n.º 11
0
        /// <summary>
        /// 滤波计算
        /// </summary>
        /// <param name="epochInfo"></param>
        /// <param name="lastResult"></param>
        /// <returns></returns>
        public override SingleSiteGnssResult CaculateKalmanFilter(EpochInformation epochInfo, SingleSiteGnssResult lastResult)
        {
            //极速模式。
            if (Option.TopSpeedModel)
            {
                return(base.CaculateKalmanFilter(epochInfo, lastResult));
            }

            epochInfo.RemoveIonoFreeUnavailable();
            if (epochInfo.Count < 2)
            {
                log.Error("卫星可用数量不足:" + epochInfo.Count);
                return(null);
            }
            var result = base.CaculateKalmanFilter(epochInfo, lastResult) as PppResult;

            //外部模糊度文件直接固定
            if (Option.IsFixingAmbiguity && Option.IsUseFixedParamDirectly && File.Exists(Option.AmbiguityFilePath) && Option.IsUsingAmbiguityFile)
            {
                return(result);
            }

            //var testBasePrn = new SatelliteNumber(12, SatelliteType.G);

            //平滑MW
            if (Option.IsFixingAmbiguity)
            {
                SmoothMwProvider.Add(epochInfo);
            }

            if (this.IsFixingAmbiguity
                //&& epochInfo.Contains(testBasePrn)
                && epochInfo.ReceiverTime.DateTime.TimeOfDay > TimeSpan.FromHours(2)
                )
            {
                //this.CurrentBasePrn = testBasePrn;

                var time  = epochInfo.ReceiverTime;
                var SD_MW = SmoothMwProvider.GetDifferMwValue(CurrentBasePrn); //周为单位
                if (SD_MW.Count < 2)                                           //MW精度不足
                {
                    return(result);
                }

                var wmFcb = WideLaneBiasService.Get(time).GetMwDiffer(CurrentBasePrn);        //用于模糊度固定,精度要求不高,一次任务获取一次即可

                SatWideNarrowValueManager wideNarrowValues = new SatWideNarrowValueManager(); //存储用于模糊度固定的宽窄项及对应的浮点产品
                //基本参数定义
                double maxMwRms      = 0.3;                                                   //MW 至少的平滑精度
                double maxDevOfInt   = 0.2;                                                   //MW 取整时允许最大的偏差
                var    f1            = Frequence.GetFrequenceA(CurrentBasePrn, time).Value;   // 1575.42;
                var    f2            = Frequence.GetFrequenceB(CurrentBasePrn, time).Value;   //  1227.60;
                var    wideWaveLen   = Frequence.GetMwFrequence(CurrentBasePrn, time).WaveLength;
                var    narrowWaveLen = Frequence.GetNarrowLaneFrequence(CurrentBasePrn, time).WaveLength;

                #region 对MW值(宽巷模糊度)进行固定
                foreach (var item in SD_MW)
                {
                    //简单的质量控制
                    if (item.Value.Rms > maxMwRms && !Double.IsNaN(item.Value.Rms))
                    {
                        continue;
                    }
                    var f = wmFcb[item.Key]; //已经归算为和WM的定义相同,在[0-1]区间,满足 B=N+f, N=B-f.
                    if (f == double.NaN)
                    {
                        continue;
                    }

                    var B = item.Value.Value;

                    var nearN = B - f;

                    var N = (int)Math.Round(nearN);                                                    //直接进行取整数
                    if (Math.Abs(nearN - N) <= maxDevOfInt)                                            //fixedSD_MW=0代表无效
                    {
                        wideNarrowValues.GetOrCreate(item.Key).WideLane = new IntFractionNumber(f, N); //改正后的 WLFCB
                    }
                }
                #endregion

                //提取浮点解星间单差
                SetPppFloatAmbiguityLen(result, wideNarrowValues);

                #region 固定窄巷模糊度                                           //星间单差,窄巷模糊度
                var tempCoeef  = f2 / (f1 + f2);                          //算法1
                var tempCoeef2 = (f1 + f2) * 1e6 / GnssConst.LIGHT_SPEED; //算法2:单位转换,窄巷波长的倒数
                var tempCoeef3 = f2 / (f1 - f2);                          //算法2
                foreach (var kv in wideNarrowValues.Data)
                {
                    //  var nFcb = SDNL_FCB[kv];//获取窄巷的产品
                    var prn     = kv.Key;
                    var satData = kv.Value;
                    var f       = NarrawLaneFcbService.GetBsdOfNarrowLane(prn, CurrentBasePrn, time); //获取窄巷的产品f, 符合B=N+f
                    if (!RmsedNumeral.IsValid(f))
                    {
                        continue;
                    }

                    var wideInt         = satData.WideLane.Int;//只需要整数部分,小数部分化为窄巷小数部分,不影响整体
                    var floatPppAmbiLen = satData.FloatAmbiguityLength;

                    var B = (floatPppAmbiLen - tempCoeef * wideWaveLen * wideInt) / narrowWaveLen;//算法1

                    #region 算法验证
                    var B2     = tempCoeef2 * floatPppAmbiLen - tempCoeef3 * wideInt;             //算法2
                    var differ = B - B2;
                    if (differ > 0.0001)
                    {
                        int ii = 0;
                        throw new Exception("Error!");
                    }
                    #endregion

                    var nearN = B - f;
                    var N     = (int)Math.Round(nearN.Value);                   //直接进行取整数
                    if (Math.Abs(nearN.Value - N) <= maxDevOfInt)               //fixedSD_MW=0代表无效
                    {
                        satData.NarrowLane = new IntFractionNumber(f.Value, N); //改正后的 WLFCB
                    }
                }
                #endregion

                //提取协方差,采用Lambda去相关后固定模糊度

                #region LAMBDA固定窄巷模糊度
                //提取已选择的模糊度协方差阵


                //#region 星间单差窄巷模糊度协方差矩阵
                //IMatrix B = new ArrayMatrix(this.Adjustment.ParamNames.Count - 6, this.Adjustment.ParamNames.Count - 5, 0.0); //将非差模糊度转换为星间单差模糊度的旋转矩阵
                //int index = 0;//参考星的索引位置???,参考星不同,index不同
                //for (int i = 5; i < this.Adjustment.ParamNames.Count; i++)
                //{
                //    SatelliteNumber sat = SatelliteNumber.Parse(this.Adjustment.ParamNames[i].Substring(0, 3));
                //    if (sat == MaxPrn)
                //    {
                //        break;
                //    }
                //    else
                //    {
                //        index++;
                //    }

                //}
                //for (int i = 0; i < this.Adjustment.ParamNames.Count - 6; i++)
                //{
                //    if (i < index)
                //    {
                //        B[i, index] = -1; B[i, i] = 1;
                //    }
                //    if (i >= index)
                //    {
                //        B[i, index] = -1; B[i, i + 1] = 1;
                //    }
                //}

                //IMatrix BT = B.Transposition;
                //IMatrix covaAmb = new ArrayMatrix(this.Adjustment.ParamNames.Count - 5, this.Adjustment.ParamNames.Count - 5, 0); //非差模糊度的协方差矩阵
                //for (int i = 0; i < this.Adjustment.ParamNames.Count - 5; i++)
                //{
                //    for (int j = 0; j < this.Adjustment.ParamNames.Count - 5; j++)
                //    {
                //        covaAmb[i, j] = this.Adjustment.CovaOfEstimatedParam[i + 5, j + 5];
                //    }
                //}
                //IMatrix SD_covaAmb = B.Multiply(covaAmb).Multiply(BT); //星间单差模糊度的协方差矩阵
                //IMatrix SDNL_covaAmb = new ArrayMatrix(this.Adjustment.ParamNames.Count - 6, this.Adjustment.ParamNames.Count - 6, 0.0); //星间单差窄巷模糊度的协方差矩阵
                //for (int i = 0; i < this.Adjustment.ParamNames.Count - 6; i++)
                //{
                //    for (int j = 0; j < this.Adjustment.ParamNames.Count - 6; j++)
                //    {
                //        SDNL_covaAmb[i, j] = SD_covaAmb[i, j] * (1575.42 + 1227.60) / (1575.42 * GnssConst.GPS_L1_WAVELENGTH);//系数
                //    }
                //}
                //#endregion

                //#region 部分模糊度固定,先将可以固定的星间单差窄巷模糊度 及其 协方差矩阵挑出来
                //List<string> old_ambpara = new List<string>();
                //foreach (var item in SD_floatAmb.Keys)
                //{
                //    old_ambpara.Add(item.ToString());//所有的模糊度(除了参考星)
                //}
                //List<string> new_ambpara = new List<string>();
                //foreach (var item in SDNL_floatAmb.Keys)
                //{
                //    new_ambpara.Add(item.ToString());//可以固定的模糊度(宽巷 < 0.25 && NLFCB != 0)
                //}
                //IMatrix newtrix = NamedMatrix.GetNewMatrix(new_ambpara, old_ambpara, SDNL_covaAmb.Array); //将可以固定的模糊度的协方差矩阵挑出来
                //#endregion



                #endregion


                StringBuilder sb = new StringBuilder();
                sb.Append("PPP模糊度固定," + time + ", BasePrn : " + CurrentBasePrn + ", 其它:" + wideNarrowValues.Count + ", " + Geo.Utils.StringUtil.ToString(wideNarrowValues.Keys));

                log.Info(sb.ToString());

                //计算固定后的模糊度距离
                //已经固定的模糊度
                var fixedPppAmbi = new Dictionary <SatelliteNumber, double>();
                foreach (var kv in wideNarrowValues.Data)
                {
                    if (!kv.Value.IsValid)
                    {
                        continue;
                    }
                    var prn             = kv.Key;
                    var satData         = kv.Value;
                    var wideInt         = satData.WideLane.Int;      //宽巷模糊度仍然采用整数
                    var floatNarrowAmbi = satData.NarrowLane.Value;  //浮点部分包括在窄巷硬件延迟中
                    var fixedAmbiLen    = tempCoeef * wideWaveLen * wideInt + narrowWaveLen * floatNarrowAmbi;

                    satData.FixedAmbiguityLength = fixedAmbiLen;
                    //简单质量控制
                    if (satData.DifferOfAmbiguityLength < 0.5)
                    {
                        fixedPppAmbi[prn] = fixedAmbiLen;
                    }
                }

                #region 条件平差  模糊度固定
                return(FixPppResult(result, fixedPppAmbi));

                #endregion
            }
            return(result);
        }