public override BaseGnssResult CaculateKalmanFilter(MultiSitePeriodInfo recInfo, BaseGnssResult lastClockEstimationResult = null)
{
DifferClockEstimationResult last = null;
if (lastClockEstimationResult != null)
{
last = (DifferClockEstimationResult)lastClockEstimationResult;
}
// ISatWeightProvider SatWeightProvider = new SatElevateAndRangeWeightProvider();
var matrixBuilder = (IonoFreeEpochDifferClockEstimationMatrixBuilder)MatrixBuilder;
matrixBuilder.CurrentMaterial = recInfo;
matrixBuilder.SetPreviousProduct(last);
matrixBuilder.Build();
// this.Adjustment = new KalmanFilter( this.MatrixBuilder);
this.Adjustment = this.RunAdjuster(BuildAdjustObsMatrix(this.CurrentMaterial));
if (Adjustment.Estimated == null)
{
return(null);
}
////尝试固定模糊度 cuiyang 2015.07
//int fixFlag = Ppp_AR.Process(recInfo, Adjustment);
DifferClockEstimationResult result = (DifferClockEstimationResult)BuildResult();
return(result);
}
/// <summary>
/// 多历元多站双差
/// </summary>
/// <param name="periodMaterial"></param>
/// <param name="BaseSiteName"></param>
/// <param name="CurrentBasePrn"></param>
/// <param name="invFactorOfPhase"></param>
/// <param name="phaseOnly"></param>
/// <param name="isDualFreq"></param>
/// <returns></returns>
public Matrix BulidDoubleInverseWeightOfObs(MultiSitePeriodInfo periodMaterial,
string BaseSiteName, SatelliteNumber CurrentBasePrn,
double invFactorOfPhase, bool phaseOnly, bool isDualFreq = false)
{
var SiteCount = periodMaterial[0].Count;
var EpochCount = periodMaterial.Count;
var EnabledSatCount = periodMaterial.EnabledSatCount;
int satCount = EnabledSatCount;
int epochObsCount = (satCount - 1); //一个历元对应的参数数量
int totalObsCount = (SiteCount - 1) * epochObsCount * (EpochCount); //原始非差观测数，only载波
if (!phaseOnly) //包含载波，翻翻
{
totalObsCount *= 2;
epochObsCount *= 2;
}
if (isDualFreq)//包含2个频率，翻翻
{
totalObsCount *= 2;
epochObsCount *= 2;
}
Matrix result = new Matrix(totalObsCount);
for (int i = 0; i < EpochCount; i++)
{
var epochMaterial = periodMaterial[i];
var subDiag = BulidDoubleInverseWeightOfObs(epochMaterial, BaseSiteName, CurrentBasePrn, invFactorOfPhase, phaseOnly, isDualFreq);
var startRowCol = epochObsCount * i;
result.SetSub(subDiag, startRowCol, startRowCol);
}
return(result);
}
/// <summary>
/// 初始化
/// </summary>
/// <param name="material"></param>
public override void Init(MultiSitePeriodInfo material)
{
if (material.EnabledPrns.Contains(Prn))
{
this.HasCycleSlip = material.HasCycleSlip(Prn);
}
}
/// <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>
/// 钟差估计构造函数
/// </summary>
/// <param name="epochInfo">历元信息</param>
/// <param name="Adjustment">平差信息</param>
/// <param name="ClockEstimationer">钟差估计器</param>
/// <param name="previousResult">上一历元结果</param>
public DifferClockEstimationResult(
MultiSitePeriodInfo epochInfo,
AdjustResultMatrix Adjustment,
GnssParamNameBuilder DifferenceClockEstimationer,
DifferClockEstimationResult previousResult = null)
: base(epochInfo, Adjustment, DifferenceClockEstimationer)
{
}
/// <summary>
/// 双差差分定位结果。具有模糊度。
/// </summary>
/// <param name="Adjustment"></param>
/// <param name="BasePrn"></param>
/// <param name="DifferPositionOption"></param>
/// <param name="PeriodDifferInfo"></param>
/// <param name="nameBuilder"></param>
public PeriodDoubleDifferPositionResult(
MultiSitePeriodInfo PeriodDifferInfo,
AdjustResultMatrix Adjustment,
SatelliteNumber BasePrn,
GnssProcessOption DifferPositionOption, GnssParamNameBuilder nameBuilder)
: base(PeriodDifferInfo, Adjustment, BasePrn, nameBuilder)
{
this.Option = DifferPositionOption;
}
/// <summary>
/// 以观测信息进行初始化
/// </summary>
/// <param name="Adjustment">平差</param>
/// <param name="BasePrn">基准星</param>
/// <param name="PeriodDifferInfo">区间差分信息</param>
/// <param name="lastPppResult">最后结果</param>
public TwoSitePeriodDifferPositionResult(
MultiSitePeriodInfo PeriodDifferInfo,
AdjustResultMatrix Adjustment,
SatelliteNumber BasePrn,
GnssParamNameBuilder nameBuilder)
: base(PeriodDifferInfo, Adjustment, nameBuilder)
{
this.BasePrn = BasePrn;
}
void pp_ProgressIncreased(BaseGnssResult e, MultiSitePeriodInfo sender)
{
BaseGnssResult result = e as BaseGnssResult;
if (result == null)
{
return;
}
_results.Add(result);
ShowCurrent(result);
}
public override BaseGnssResult CaculateIndependent(MultiSitePeriodInfo epochInfos)
{
if (!this.MatrixBuilder.IsAdjustable)
{
log.Warn("不适合平差！" + MatrixBuilder.Message); return(null);
}
this.Adjustment = this.RunAdjuster(BuildAdjustObsMatrix(this.CurrentMaterial));
AdjustResultMatrix lastFloat = this.Adjustment;
return(BuildResult());
}
public virtual void Init(MultiSitePeriodInfo epochInfo)
{
this.Init(epochInfo.First);
}
/// <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);
}
