Пример #1
0
        /// <summary>
        /// 算后结果处理,主要检查方差是否过大,剔除陡峭数据
        /// </summary>
        /// <param name="product">本次历元计算结果</param>
        /// <returns>如果为null,则表示计算失败,或结果被剔除</returns>
        public override bool CheckProduct(TProduct product)
        {
            if (product is BaseGnssResult)
            {
                BaseGnssResult ppResult = product as BaseGnssResult;
                if (ppResult == null)
                {
                    return(false);
                }

                //对解算结果进行评价,决定计算结果是否有效。
                if (!AdjustChecker.Check(ppResult))
                {
                    //throw checker.Exception;
                    log.Info(ppResult.Name + " 计算结果未通过检核!在:" + ppResult + ", " + AdjustChecker.Exception.Message);
                    return(false);
                }
            }
            return(true);
        }
Пример #2
0
        /// <summary>
        /// 获取失败的卫星,具有周跳的,已经进行了降权,因此不返回。
        /// </summary>
        /// <returns></returns>
        public List <SatelliteNumber> GetBadPrns(BaseGnssResult product)
        {
            List <SatelliteNumber> badPrns = new List <SatelliteNumber>();
            var csedPrns = product.Material.UnstablePrns;//忽略周跳卫星的检查


            var postResiduals = product.ResultMatrix.PostfitResidual;

            if (product is NetDoubleDifferPositionResult || product is DualSiteEpochDoubleDifferResult)
            {
                postResiduals = postResiduals.FilterContains(ParamNames.DoubleDiffer + ParamNames.PhaseL);

                var collection = GetBadElementResidual(postResiduals);

                foreach (var item in collection)
                {
                    var name = NetDoubleDifferName.Parse(item);
                    AddToBadPrns(badPrns, csedPrns, name.RovPrn);
                }
            }
            else if (product.Material is EpochInformation)
            {
                var collection = GetBadElementResidual(postResiduals);
                foreach (var item in collection)
                {
                    SatelliteNumber prn = SatelliteNumber.Parse(item);
                    if (prn.PRN == 0)
                    {
                        continue;
                    }

                    AddToBadPrns(badPrns, csedPrns, prn);
                }
            }


            return(badPrns);
        }
        /// <summary>
        /// Kalmam滤波。
        /// </summary>
        /// <param name="MultiSitePeriodInfo">接收信息</param>
        /// <param name="lastResult">上次解算结果(用于 Kalman 滤波),若为null则使用初始值计算</param>
        /// <returns></returns>
        public override BaseGnssResult CaculateKalmanFilter(PeriodInformation MultiSitePeriodInfo, BaseGnssResult lastResult = null)
        {
            if (!this.MatrixBuilder.IsAdjustable)
            {
                log.Debug("不适合平差!" + MatrixBuilder.Message);
                return(null);
            }

            // this.Adjustment = new KalmanFilter(MatrixBuilder);
            this.Adjustment = this.RunAdjuster(BuildAdjustObsMatrix(this.CurrentMaterial));

            //固定解
            return(BuildResult());
        }
Пример #4
0
        /// <summary>
        /// Kalmam滤波。
        /// </summary>
        /// <param name="epochInfos">接收信息</param>
        /// <param name="lastResult">上次解算结果(用于 Kalman 滤波),若为null则使用初始值计算</param>
        /// <returns></returns>
        public override BaseGnssResult CaculateKalmanFilter(MultiSiteEpochInfo epochInfos, BaseGnssResult lastResult = null)
        {
            var result = base.CaculateKalmanFilter(epochInfos, lastResult) as EpochDoubleDifferPositionResult;

            //这里其实不用了,顶层已经做了
            if (Option.PositionType == PositionType.动态定位 && Option.IsUpdateEstimatePostition)
            {
                epochInfos.OtherEpochInfo.SiteInfo.EstimatedXyz = result.GetEstimatedBaseline().EstimatedXyzOfRov;
            }

            this.SetProduct(result);

            return(result);
        }
Пример #5
0
        /// <summary>
        /// Kalmam滤波。
        /// </summary>
        /// <param name="epochInfos">接收信息</param>
        /// <param name="lastResult">上次解算结果(用于 Kalman 滤波),若为null则使用初始值计算</param>
        /// <returns></returns>
        public override BaseGnssResult CaculateKalmanFilter(MultiSiteEpochInfo epochInfos, BaseGnssResult lastResult = null)
        {
            var result = base.CaculateKalmanFilter(epochInfos, lastResult) as EpochDouFreDoubleDifferPositionResult;

            //模糊度固定解
            if (this.IsFixingAmbiguity)// && result.EstimatedXyzRms.Norm < 0.001)
            {
                WeightedVector fixedIntAmbiCycles = null;
                if (!TryFixeAmbiguites(result, out fixedIntAmbiCycles))
                {
                    return(result);
                }

                //固定成功,则将浮点解作为虚拟观测值,整数解作为约束进行条件平差
                //保存到结果,用于输出
                result.FixedIntAmbiguities = fixedIntAmbiCycles;

                var lastDdResult = lastResult as EpochDouFreDoubleDifferPositionResult;
                if (lastDdResult == null)
                {
                    return(result);
                }
                ////恢复模糊度为米,计算固定解
                //var fixedAmbiMeters = ConvertCycleAmbiguityToMeter(fixedIntAmbiCycles);
                //var prevfixedAmbiMeters = ConvertCycleAmbiguityToMeter( lastDdResult.FixedIntAmbiguities);
                //WeightedVector NewEstimated = Adjustment.SolveAmbiFixedResult(fixedAmbiMeters, prevfixedAmbiMeters);

                WeightedVector NewEstimated = Adjustment.SolveAmbiFixedResult(fixedIntAmbiCycles, lastDdResult.FixedIntAmbiguities, Option.IsFixParamByConditionOrHugeWeight);

                //

                int nx = result.ResultMatrix.Estimated.Count; //参数个数  n*1
                // int nx = 5; //参数个数  n*1
                //set solution
                for (int i = 0; i < nx; i++)
                {
                    result.ResultMatrix.Estimated[i] = NewEstimated[i];
                    //for (j = 0; j < nx; j++)
                    //{
                    //    if (i == j)
                    //    {
                    //        if (Estimated.InverseWeight[i, j] == 0.0 && i >= 5)  //如果取0,后续无法直接求逆
                    //        {
                    //            adjustment.Estimated.InverseWeight[i, i] = 1e-14;
                    //        }
                    //    }
                    //    else
                    //    {
                    //        adjustment.Estimated.InverseWeight[i, j] = Estimated.InverseWeight[i, j];
                    //    }
                    //}
                }
                //for (int i = 0; i < fixedIntAmbiCycles.Count; i++)
                //{
                //    //对所有的参数
                //    var name = fixedIntAmbiCycles.ParamNames[i];
                //    int j = result.Adjustment.ParamNames.IndexOf(name);

                //    //此处应该判断,如果是第一次出现,
                //    //第二次出现且和以往的模糊度不冲突的情况下再固定模糊度。
                //    //增加高权
                //    //result.Adjustment.Estimated.InverseWeight.SetColValue(j, 0);
                //    //result.Adjustment.Estimated.InverseWeight.SetRowValue(j, 0);
                //    result.Adjustment.Estimated.InverseWeight[j, j] = 1e-20;
                //}

                // result.Adjustment.Estimated = NewEstimated;
            }

            this.SetProduct(result);

            return(result);
        }
Пример #6
0
        public override BaseGnssResult CaculateKalmanFilter(MultiSiteEpochInfo epochInfo, BaseGnssResult last = null)
        {
            var result = base.CaculateKalmanFilter(epochInfo, last);

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

            return(result);
        }
Пример #7
0
        /// <summary>
        /// PPP 计算核心方法。 Kalmam滤波。
        /// 观测量的顺序是先伪距观测量,后载波观测量,观测量的总数为卫星数量的两倍。
        /// 参数数量为卫星数量加5,卫星数量对应各个模糊度,5为3个位置量xyz,1个接收机钟差量,1个对流程湿分量。
        /// </summary>
        /// <param name="mSiteEpochInfo">接收信息</param>
        /// <param name="lastResult">上次解算结果(用于 Kalman 滤波),若为null则使用初始值计算</param>
        /// <returns></returns>
        public override BaseGnssResult CaculateKalmanFilter(MultiSiteEpochInfo mSiteEpochInfo, BaseGnssResult lastResult = null)
        {
            var refEpoch = mSiteEpochInfo.BaseEpochInfo;
            var rovEpoch = mSiteEpochInfo.OtherEpochInfo;

            IonFreeDoubleDifferPositionResult last = null;

            if (lastResult != null)
            {
                last = (IonFreeDoubleDifferPositionResult)lastResult;
            }

            //双差必须观测到2颗以上卫星,否则返回空
            if (refEpoch.EnabledPrns.Count < 2 && rovEpoch.EnabledPrns.Count < 2)
            {
                log.Warn("双差必须观测到2颗以上卫星,返回空");
                return(null);
            }

            this.Adjustment = this.RunAdjuster(BuildAdjustObsMatrix(mSiteEpochInfo));

            if (Adjustment.Estimated == null)
            {
                return(null);
            }
            this.Adjustment.ResultType = ResultType.Float;


            //检查并尝试固定模糊度

            if (true)//默认算法
            {
                var theResult = BuildResult();
                theResult.ResultMatrix.ResultType = ResultType.Float;

                //检查并尝试固定模糊度
                theResult = this.CheckOrTryToGetAmbiguityFixedResult(theResult);

                return(theResult);
            }

            //-----cuiyang 算法 --2018.12.27-------------模糊度固定-----------------------------
            if (false && Option.IsFixingAmbiguity)
            {
                //尝试固定模糊度
                lock (locker)
                {
                    //尝试固定模糊度
                    var fixIonFreeAmbiCycles = DoubleIonFreeAmReslution.Process(rovEpoch, refEpoch, Adjustment, CurrentBasePrn);
                    //是否保存固定的双差LC模糊度信息,继承已固定的? 建议不用, 始终动态更新
                    // 对过去已固定,但当前历元却没有固定,是否继承下来?根据是否发生基准星变化、周跳等信息判断
                    WeightedVector preFixedVec = new WeightedVector(); //上一次固定结果,这里好像没有用,//2018.07.31, czs

                    //作为约束条件,重新进行平差计算
                    if (fixIonFreeAmbiCycles != null && fixIonFreeAmbiCycles.Count > 0)//
                    {
                        fixIonFreeAmbiCycles.InverseWeight = new Matrix(new DiagonalMatrix(fixIonFreeAmbiCycles.Count, 1e-10));

                        WeightedVector NewEstimated = Adjustment.SolveAmbiFixedResult(fixIonFreeAmbiCycles, preFixedVec, Option.IsFixParamByConditionOrHugeWeight);

                        XYZ newXYZ = new XYZ(NewEstimated[0], NewEstimated[1], NewEstimated[2]);
                        XYZ oldXYZ = new XYZ(Adjustment.Estimated[0], Adjustment.Estimated[1], Adjustment.Estimated[2]);
                        //模糊度固定错误的判断准则(参考李盼论文)
                        if ((newXYZ - oldXYZ).Length > 0.05 && newXYZ.Length / oldXYZ.Length > 1.5)
                        {
                            //模糊度固定失败,则不用
                        }
                        else
                        {
                            int nx = Adjustment.Estimated.Count;
                            //nx = 3;
                            for (int i = 0; i < nx; i++)
                            {
                                Adjustment.Estimated[i] = NewEstimated[i];
                                // for (int j = 0; j < nx ; j++) adjustment.Estimated.InverseWeight[i, j] = Estimated.InverseWeight[i, j];
                            }
                            this.Adjustment.ResultType = ResultType.Fixed;
                        }
                    }
                }
            }

            if (Adjustment.Estimated != null)
            {
                var result = BuildResult();
                this.SetProduct(result);
                //是否更新测站估值
                this.CheckOrUpdateEstimatedCoord(mSiteEpochInfo, result);
                return(result);
            }
            else
            {
                return(null);
            }
        }
        /// <summary>
        /// PPP 计算核心方法。 Kalmam滤波。
        /// 观测量的顺序是先伪距观测量,后载波观测量,观测量的总数为卫星数量的两倍。
        /// 参数数量为卫星数量加5,卫星数量对应各个模糊度,5为3个位置量xyz,1个接收机钟差量,1个对流程湿分量。
        /// </summary>
        /// <param name="mSiteEpochInfo">接收信息</param>
        /// <param name="lastResult">上次解算结果(用于 Kalman 滤波),若为null则使用初始值计算</param>
        /// <returns></returns>
        public override BaseGnssResult CaculateKalmanFilter(MultiSiteEpochInfo mSiteEpochInfo, BaseGnssResult lastResult = null)
        {
            //return base.CaculateKalmanFilter(mSiteEpochInfo, lastResult);

            var refEpoch = mSiteEpochInfo.BaseEpochInfo;
            var rovEpoch = mSiteEpochInfo.OtherEpochInfo;

            IonFreeDoubleDifferPositionResult last = null;

            if (lastResult != null)
            {
                last = (IonFreeDoubleDifferPositionResult)lastResult;
            }

            //双差必须观测到2颗以上卫星,否则返回空
            if (refEpoch.EnabledPrns.Count < 2 && rovEpoch.EnabledPrns.Count < 2)
            {
                return(null);
            }

            //随机噪声模型合适否??????

            //DualBandCycleSlipDetector.Dector(ref recInfo, ref currentRefInfo, CurrentBasePrn);

            //this.Adjustment = new KalmanFilter(MatrixBuilder);
            this.Adjustment = this.RunAdjuster(BuildAdjustObsMatrix(this.CurrentMaterial));//         BuildAdjustObsMatrix(this.CurrentMaterial));

            if (Adjustment.Estimated == null)
            {
                return(null);
            }
            this.Adjustment.ResultType = ResultType.Float;

            ////验后残差分析,探测是否漏的周跳或粗差
            //bool isCS = ResidualsAnalysis.Detect(ref refEpoch, ref rovEpoch, Adjustment, CurrentBasePrn);

            //while (isCS)
            //{
            //    if (refEpoch.EnabledSatCount > 4) //1个基准星+n
            //    {
            //        this.MatrixBuilder = new IonFreeDoubleDifferMatrixBuilder(Option, BaseParamCount);
            //        //  this.MatrixBuilder.SetEpochInfo(recInfo).SetPreviousResult(lastPppResult);


            //        this.CurrentMaterial.BaseEpochInfo = refEpoch;
            //        this.CurrentMaterial.OtherEpochInfo = rovEpoch;
            //        this.MatrixBuilder.SetMaterial(this.CurrentMaterial).SetPreviousProduct(this.CurrentProduct);
            //        this.MatrixBuilder.SetBasePrn(this.CurrentBasePrn);
            //        this.MatrixBuilder.Build();

            //        //this.Adjustment = new KalmanFilter(MatrixBuilder);
            //        //this.Adjustment = new SimpleKalmanFilterOld(MatrixBuilder);
            //        //this.Adjustment.Process();
            //        this.Adjustment = this.RunAdjuster(BuildAdjustObsMatrix(this.CurrentMaterial));
            //        if (Adjustment.Estimated == null)
            //        {
            //            return null;
            //        }
            //        isCS = ResidualsAnalysis.Detect(ref refEpoch, ref rovEpoch, Adjustment, CurrentBasePrn);
            //    }
            //    else
            //    { isCS = false; }
            //}

            //模糊度固定解
            if (Option.IsFixingAmbiguity)
            {
                //尝试固定模糊度
                lock (locker)
                {
                    //尝试固定模糊度
                    var fixIonFreeAmbiCycles = DoubleIonFreeAmReslution.Process(rovEpoch, refEpoch, Adjustment, CurrentBasePrn);
                    //是否保存固定的双差LC模糊度信息,继承已固定的? 建议不用, 始终动态更新
                    // 对过去已固定,但当前历元却没有固定,是否继承下来?根据是否发生基准星变化、周跳等信息判断
                    WeightedVector preFixedVec = new WeightedVector(); //上一次固定结果,这里好像没有用,//2018.07.31, czs

                    //作为约束条件,重新进行平差计算
                    if (fixIonFreeAmbiCycles != null && fixIonFreeAmbiCycles.Count > 0)//
                    {
                        fixIonFreeAmbiCycles.InverseWeight = new Matrix(new DiagonalMatrix(fixIonFreeAmbiCycles.Count, 1e-10));

                        WeightedVector NewEstimated = Adjustment.SolveAmbiFixedResult(fixIonFreeAmbiCycles, preFixedVec, Option.IsFixParamByConditionOrHugeWeight);

                        XYZ newXYZ = new XYZ(NewEstimated[0], NewEstimated[1], NewEstimated[2]);
                        XYZ oldXYZ = new XYZ(Adjustment.Estimated[0], Adjustment.Estimated[1], Adjustment.Estimated[2]);
                        //模糊度固定错误的判断准则(参考李盼论文)
                        if ((newXYZ - oldXYZ).Length > 0.05 && newXYZ.Length / oldXYZ.Length > 1.5)
                        {
                            //模糊度固定失败,则不用
                        }
                        else
                        {
                            int nx = Adjustment.Estimated.Count;
                            //nx = 3;
                            for (int i = 0; i < nx; i++)
                            {
                                Adjustment.Estimated[i] = NewEstimated[i];
                                // for (int j = 0; j < nx ; j++) adjustment.Estimated.InverseWeight[i, j] = Estimated.InverseWeight[i, j];
                            }
                        }
                    }
                    this.Adjustment.ResultType = ResultType.Fixed;
                    //替换固定的模糊度参数,重新平差,依然不对啊
                    #region 参考Ge论文给的Blewitt 1989论文思路
                    //if (fixIonFreeAmbiCycles.Count > 1)
                    //{
                    //    Vector newAprioriParam = this.MatrixBuilder.AprioriParam;
                    //    IMatrix newAprioriParamCovInverse = this.MatrixBuilder.AprioriParam.InverseWeight.Inversion;

                    //    for (int i = 0; i < fixIonFreeAmbiCycles.Count; i++)
                    //    {
                    //        //对所有的参数
                    //        var name = fixIonFreeAmbiCycles.ParamNames[i];
                    //        int j = this.MatrixBuilder.ParamNames.IndexOf(name);
                    //        newAprioriParam[j] = fixIonFreeAmbiCycles[i];
                    //        newAprioriParamCovInverse[j, j] = 1e28;
                    //    }

                    //    IMatrix TransferMatrix = MatrixBuilder.Transfer;
                    //    IMatrix AprioriParam = new Matrix(newAprioriParam);
                    //    IMatrix CovaOfAprioriParam = newAprioriParamCovInverse.GetInverse();
                    //    IMatrix InverseWeightOfTransfer = MatrixBuilder.Transfer.InverseWeight;
                    //    IMatrix PredictParam = TransferMatrix.Multiply(AprioriParam);//.Plus(controlMatrix.Multiply(controlInputVector));
                    //    IMatrix CovaOfPredictParam1 = AdjustmentUtil.BQBT(TransferMatrix, CovaOfAprioriParam).Plus(InverseWeightOfTransfer);

                    //    //简化字母表示
                    //    Matrix Q1 = new Matrix(CovaOfPredictParam1);
                    //    Matrix P1 = new Matrix(CovaOfPredictParam1.GetInverse());
                    //    Matrix X1 = new Matrix(PredictParam);
                    //    Matrix L = new Matrix(MatrixBuilder.Observation.Array);
                    //    Matrix A = new Matrix(MatrixBuilder.Coefficient);
                    //    Matrix AT = new Matrix(A.Transposition);
                    //    Matrix Po = new Matrix(MatrixBuilder.Observation.InverseWeight.GetInverse());
                    //    Matrix Qo = new Matrix(MatrixBuilder.Observation.InverseWeight);
                    //    Matrix ATPA = new Matrix(AdjustmentUtil.ATPA(A, Po));
                    //    //平差值Xk的权阵
                    //    Matrix PXk = null;
                    //    if (Po.IsDiagonal) { PXk = ATPA + P1; }
                    //    else { PXk = AT * Po * A + P1; }

                    //    //计算平差值的权逆阵
                    //    Matrix Qx = PXk.Inversion;
                    //    Matrix J = Qx * AT * Po;

                    //    //计算平差值
                    //    Matrix Vk1 = A * X1 - L;//计算新息向量
                    //    Matrix X = X1 - J * Vk1;
                    //    var Estimated = new WeightedVector(X, Qx) { ParamNames = MatrixBuilder.Observation.ParamNames };
                    //    WeightedVector NewAdjustment = Estimated;
                    //    int nx = Adjustment.Estimated.Count;
                    //    for (int i = 0; i < nx; i++)
                    //    {
                    //        Adjustment.Estimated[i] = NewAdjustment[i];
                    //        // for (int j = 0; j < nx ; j++) adjustment.Estimated.InverseWeight[i, j] = Estimated.InverseWeight[i, j];
                    //    }
                    //}

                    #endregion

                    //替换固定的模糊度参数,重新平差,依然不对啊
                    #region 参考Dong 1989论文方法
                    //if (fixIonFreeAmbiCycles.Count > 0)
                    //{
                    //    WeightedVector estIonFreeAmbiVector = this.Adjustment.Estimated.GetWeightedVector(fixIonFreeAmbiCycles.ParamNames);
                    //    List<string> paramNames = new List<string>();
                    //    foreach (var item in this.Adjustment.Estimated.ParamNames)
                    //    {
                    //        if (!fixIonFreeAmbiCycles.ParamNames.Contains(item))
                    //        {
                    //            paramNames.Add(item);
                    //        }
                    //    }
                    //    foreach (var item in fixIonFreeAmbiCycles.ParamNames) paramNames.Add(item);


                    //    var Estimate = this.Adjustment.Estimated;

                    //    var orderEstimate = Estimate.SortByName(paramNames);

                    //    Matrix order = new Matrix(paramNames.Count, paramNames.Count);
                    //    for(int i=0;i<paramNames.Count;i++)
                    //    {
                    //        int j = Estimate.ParamNames.IndexOf(paramNames[i]);
                    //        order[i, j] = 1;
                    //    }

                    //    Matrix X1 = new Matrix(Estimate.Array);
                    //    Matrix QX1 = new Matrix(Estimate.InverseWeight);

                    //    Matrix newX1 = order * X1;
                    //    Matrix newX1Cov = order * QX1 * order.Transpose();

                    //    int n1 = Estimate.ParamNames.Count - fixIonFreeAmbiCycles.Count;
                    //    Matrix Q12 = newX1Cov.GetSub(0, n1, n1, fixIonFreeAmbiCycles.Count);
                    //    Matrix Q22 = newX1Cov.GetSub(n1, n1, fixIonFreeAmbiCycles.Count, fixIonFreeAmbiCycles.Count);

                    //    Matrix detX2 = new Matrix(fixIonFreeAmbiCycles.Count,1);
                    //    for(int i=0;i<fixIonFreeAmbiCycles.Count;i++)
                    //    {
                    //        int j = Estimate.ParamNames.IndexOf(fixIonFreeAmbiCycles.ParamNames[i]);
                    //        detX2[i, 0] = fixIonFreeAmbiCycles[i] - Estimate.Data[j];

                    //    }

                    //    Matrix X = Q12 * Q22.Inversion * detX2;

                    //    Vector newX = new Vector();
                    //    for (int i = 0; i < X.RowCount; i++) newX.Add(X[i, 0], paramNames[i]);
                    //    for (int i = 0; i < fixIonFreeAmbiCycles.Count; i++) newX.Add(detX2[i,0], fixIonFreeAmbiCycles.ParamNames[i]);

                    //    WeightedVector newEstrimate = new WeightedVector(newX);
                    //    newEstrimate.ParamNames = paramNames;


                    //    int nx = Adjustment.Estimated.Count;
                    //    for (int i = 0; i < 3; i++)
                    //    {
                    //        int j = newEstrimate.ParamNames.IndexOf(Adjustment.Estimated.ParamNames[i]);
                    //        Adjustment.Estimated[i] += newEstrimate[j];
                    //        // for (int j = 0; j < nx ; j++) adjustment.Estimated.InverseWeight[i, j] = Estimated.InverseWeight[i, j];
                    //    }
                    //}
                    #endregion
                }
            }

            if (Adjustment.Estimated != null)
            {
                var DDResidualDifferPositionResult = BuildResult();


                //if (Option.PositionType == PositionType.动态定位)
                //{
                //    mSiteEpochInfo.OtherEpochInfo.SiteInfo.EstimatedXyz = ((IonFreeDoubleDifferPositionResult)DDResidualDifferPositionResult).EstimatedXyzOfRov;
                //}

                //double[] v = PppResidualDifferPositionResult.Adjustment.PostfitResidual.OneDimArray;

                //int k = recInfo.EnabledPrns.IndexOf(CurrentBasePrn);

                //for (int i = 0; i < recInfo.EnabledSatCount; i++)
                //{
                //    SatelliteNumber prn = recInfo[i].Prn;

                //    if (prn != CurrentBasePrn)
                //    {
                //        List<double> tmp = new List<double>();
                //        if (!posfit.ContainsKey(prn)) posfit.Add(prn, tmp);

                //        if (i < k)
                //        {
                //            posfit[prn].Add(v[i + recInfo.EnabledSatCount - 1]);
                //        }
                //        else
                //        { posfit[prn].Add(v[i - 1 + recInfo.EnabledSatCount - 1]); }
                //    }
                //}

                //   this.SetProduct(DDResidualDifferPositionResult);


                return(DDResidualDifferPositionResult);
            }
            else
            {
                return(null);
            }
        }
Пример #9
0
        /// <summary>
        /// 滤波算法。
        /// </summary>
        /// <param name="epochInfos"></param>
        /// <param name="lastResult"></param>
        /// <returns></returns>
        public override BaseGnssResult CaculateKalmanFilter(MultiSitePeriodInfo epochInfos, BaseGnssResult lastResult = null)
        {
            var result = base.CaculateKalmanFilter(epochInfos, lastResult) as PeriodDoubleDifferPositionResult;


            //模糊度固定解
            //if (Option.IsFixingAmbiguity && result.EstimatedXyzRms.Norm < 0.01)
            //{
            //    Vector fixedIntAmbiCycles = null;
            //    if (!TryFixeAmbiguites(result, out fixedIntAmbiCycles))
            //    {
            //        return result;
            //    }

            //    //固定成功,则将浮点解作为虚拟观测值,整数解作为约束进行条件平差
            //    //保存到结果,用于输出
            //    result.FixedIntAmbiguities = fixedIntAmbiCycles;
            //    var lastDdResult = lastResult as PeriodDoubleDifferPositionResult;
            //    if (lastDdResult == null)
            //    {
            //        return result;
            //    }
            //    //恢复模糊度为米,计算固定解
            //    var fixedAmbiMeters = ConvertCycleAmbiguityToMeter(fixedIntAmbiCycles);
            //    var prevFixedAmbiMeters = ConvertCycleAmbiguityToMeter(lastDdResult.FixedIntAmbiguities);
            //    WeightedVector NewEstimated = Adjustment.SolveAmbiFixedResult(fixedAmbiMeters, prevFixedAmbiMeters);

            //    result.ResultMatrix.Estimated = NewEstimated;
            //}

            //this.CurrentProduct = result;

            return(result);
        }
Пример #10
0
        /// <summary>
        /// Kalmam滤波。
        /// </summary>
        /// <param name="epochInfos">接收信息</param>
        /// <param name="lastResult">上次解算结果(用于 Kalman 滤波),若为null则使用初始值计算</param>
        /// <returns></returns>
        public BaseGnssResult CaculateKalmanFilter2(MultiSitePeriodInfo epochInfos, BaseGnssResult lastResult = null)
        {
            var result = base.CaculateKalmanFilter(epochInfos, lastResult) as PeriodDoubleDifferPositionResult;

            //模糊度固定解
            if (this.IsFixingAmbiguity)
            {
                AmbiguityManager.Regist(result); //用于存储和输出模糊度。

                var floatSolMetter = this.Adjustment.Estimated;
                //尝试固定模糊度
                var fixedIntAmbiCycles = FixePhaseAmbiguity(result);

                //模糊度固定失败,直接返回浮点数结果。
                if (fixedIntAmbiCycles.Count == 0)
                {
                    return(result);
                }

                //固定成功,则将浮点解作为虚拟观测值,整数解作为约束进行条件平差

                //保存到结果,用于输出
                result.FixedParams = fixedIntAmbiCycles;
                //恢复模糊度为米
                var fixedAmbiMeters = ConvertCycleAmbiguityToMeter(fixedIntAmbiCycles);

                //组建条件数的系数阵,
                var coeffOfConstrant = BuildCoeffiientMatrix(floatSolMetter.ParamNames, fixedAmbiMeters.ParamNames);

                //条件平差,观测值为浮点解,上一观测结果作为虚拟观测值,
                var obsVector = floatSolMetter;
                var ca        = new ConditionalAdjustment(obsVector, coeffOfConstrant, (Vector)(fixedAmbiMeters * (-1.0))); //此处B0为加,所以要乘以-1
                ca.Process();

                var fixedDiffer = ca.CorrectedObservation;
                //避免方差太小,但是好像没有什么用处。
                for (int j = 0; j < floatSolMetter.Count; j++)
                {
                    if (fixedDiffer.InverseWeight[j, j] < 1e-6)
                    {
                        fixedDiffer.InverseWeight[j, j] = 1e-6;
                    }
                }


                //更新结果
                //只更新坐标
                //floatSolMetter[Gnsser.ParamNames.DeltaX] = fixedDiffer[Gnsser.ParamNames.DeltaX];
                //floatSolMetter[Gnsser.ParamNames.DeltaY] = fixedDiffer[Gnsser.ParamNames.DeltaY];
                //floatSolMetter[Gnsser.ParamNames.DeltaZ] = fixedDiffer[Gnsser.ParamNames.DeltaZ];
                result.ResultMatrix.Estimated = fixedDiffer;

                //result.Adjustment.Estimated = estimated;

                //from cuiyang to check
                IMatrix B      = coeffOfConstrant;
                IMatrix BT     = B.Transposition;
                IMatrix L      = floatSolMetter;
                IMatrix QL     = floatSolMetter.InverseWeight;
                IMatrix B0     = new VectorMatrix(fixedAmbiMeters * (-1)); //条件方程常数项
                IMatrix W      = B.Multiply(L).Plus(B0);                   // W = BL - B0
                IMatrix BQBT   = B.Multiply(QL).Multiply(BT);
                IMatrix Nadd   = (QL.Multiply(BT)).Multiply(BQBT.GetInverse());
                IMatrix X_new  = L.Minus(Nadd.Multiply(W));
                IMatrix QX_new = QL.Minus(Nadd.Multiply(B).Multiply(QL));
                var     check  = new WeightedVector(X_new, QX_new);
                int     ii     = 0;
                //result.Adjustment.Estimated = new WeightedVector(X_new, QX_new);
            }

            this.SetProduct(result);

            return(result);
        }