/// <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);
}
/// <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());
}
/// <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);
}
/// <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);
}
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);
}
/// <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);
}
}
/// <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);
}
/// <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);
}
