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