/// <summary>
/// 执行无电离层双差模糊度固定
/// </summary>
/// <param name="rawFloatAmbiCycles"></param>
/// <param name="isNetSolve">是否网解</param>
/// <returns></returns>
protected WeightedVector DoFixIonoFreeAmbiguity(WeightedVector rawFloatAmbiCycles, bool isNetSolve)
{
//-----------以下为无电离层组合模糊度固定算法--------------------
if (this.DataSourceContext.SiteSatPeriodDataService == null)
{
log.Warn("必须开启时段数据服务，才能实现无电离层模糊度固定！");
return(new WeightedVector());
}
var time = this.CurrentMaterial.ReceiverTime;
//指定系统的无电离层组合参数计算器
var IonoFreeAmbiguitySolver = IonoFreeAmbiguitySolverManager.GetOrCreate(CurrentBasePrn.SatelliteType);
IonoFreeAmbiguitySolver.CheckOrInit(CurrentBasePrn, CurrentMaterial.ReceiverTime, !Option.IsLengthPhaseValue);
//----------------------第一步 MW 宽巷星间单差 ------------------------
var intMwDoubleDiffers = GetIntMwDiffersBeweenSat(isNetSolve);
//----------------------第二步 MW 宽巷和模糊度浮点解求窄巷模糊度--------
var ambiFloatVal = rawFloatAmbiCycles.GetNameRmsedNumeralVector();
//求窄巷模糊度浮点解//单位周
var narrowFloat = IonoFreeAmbiguitySolver.GetNarrowFloatValue(intMwDoubleDiffers, ambiFloatVal);
//--------记录小数偏差部分-------
var satPeriodService = this.DataSourceContext.SiteSatPeriodDataService.Get(this.CurrentMaterial.SiteName);
foreach (var item in narrowFloat)
{
var prn = SatelliteNumber.Parse(item.Key);
satPeriodService.GetOrCreate(prn).Regist(CurrentBasePrn + "-" + ParamNames.NarrowLaneBsdCycle, time, item.Value.Value, this.CurrentMaterial[prn].IsUnstable);
}
return(new WeightedVector());
}
private void button_run_Click(object sender, EventArgs e)
{
var floatPath = fileOpenControl_ionoFreeFloat.FilePath;
var mwPath = this.fileOpenControl_mwWide.FilePath;
var mwTable = ObjectTableReader.Read(mwPath);
var floatTable = ObjectTableReader.Read(floatPath);
//Group 文件包括， Index Site Name Group Value Rms
//参数文件包括，ParamName,Value, Rms
//首先求取宽项模糊度
GroupedValueService wmValues = new GroupedValueService(mwTable);
ParamValueService paramValueService = new ParamValueService(floatTable);
NameRmsedNumeralVector wideLaneVector = GetIntMwDoubleDiffers(paramValueService, wmValues);
WeightedVector rawFloatAmbiCycles = paramValueService.GetWeightedVector();
WeightedVector fixedParams = DoFixIonoFreeDoubleDifferAmbiguity(rawFloatAmbiCycles, wideLaneVector);
//输出结果
ObjectTableStorage fixedParam = BuildFixedParamTable(fixedParams);
objectTableControl1.DataBind(fixedParam);
var path = Path.Combine(directorySelectionControl1.Path, "FixedParams" + Setting.AmbiguityFileExtension);
ObjectTableWriter.Write(fixedParam, path);
Geo.Utils.FormUtil.ShowOkAndOpenDirectory(path);
}
/// <summary>
/// 默认采用Lambda算法直接固定。
/// 如果是无电离层组合，则需要分别对待，不能直接固定，需要子类进行实现，//2018.11.06，czs， hmx
/// </summary>
/// <param name="rawFloatAmbiCycles"></param>
/// <returns></returns>
protected virtual WeightedVector DoFixAmbiguity(WeightedVector rawFloatAmbiCycles)
{
//实时固定，采用Lambda算法，按照权逆阵排序，大的在后面，如果失败后，则删除之
var orderedFloatAmbiCycles = rawFloatAmbiCycles.GetCovaOrdered();
return(Gnsser.LambdaAmbiguitySearcher.GetAmbiguity(orderedFloatAmbiCycles, 3.0, 1e-20));
}
/// <summary>
/// 获取小数部分。
/// 整数项无关获取小数法。
/// </summary>
/// <param name="vector">带权向量</param>
/// <returns></returns>
public static double GetIntFreeFraction(WeightedVector vector)
{
return(Geo.Utils.DoubleUtil.GetIntFreeFraction(vector.OneDimArray, vector.GetWeightVector()));
double totalUp = 0;
double totalDown = 0;
double weight = 1.0;
int length = vector.Count;
bool hasWeight = vector.IsWeighted;
for (int i = 0; i < length; i++)
{
if (hasWeight)
{
weight = vector.GetWeithValue(i);
}
double floatNum = vector[i];
double twoPiMulti = 2.0 * GeoConst.PI * floatNum;
double up = weight * Math.Sin(twoPiMulti);
double down = weight * Math.Cos(twoPiMulti);
totalUp += up;
totalDown += down;
}
double val = Math.Atan2(totalUp, totalDown);
return(val);
}
public LambdaAmbiguitySearcher(WeightedVector floatSolution, int fixingCount = 2)
{
this.n = floatSolution.Count;
this.Qa = floatSolution.InverseWeight.Array;
this.a = floatSolution.OneDimArray;
this.m = fixingCount;
MaxRatio = 3;
}
private WeightedVector fix_sol(EpochInformation receiverInfo, AdjustResultMatrix adjustment, int[] satN1, int[] satN2, double[] NC, int n, ref double factor0)
{
int i, j, k;
double[] v = new double[n]; //m * 1
int nx = adjustment.Estimated.Count; //参数个数 n*1
double[][] H = new double[nx][]; for (i = 0; i < nx; i++)
{
H[i] = new double[n]; //n * m
}
double[][] R = new double[n][]; for (i = 0; i < n; i++)
{
R[i] = new double[n]; //m * m
}
SatelliteNumber sat1 = new SatelliteNumber();
SatelliteNumber sat2 = new SatelliteNumber();
//constraints to fixed ambiguities
for (i = 0; i < n; i++)
{
sat1.PRN = satN1[i];
sat1.SatelliteType = SatelliteType.G;
sat2.PRN = satN2[i];
sat2.SatelliteType = SatelliteType.G;
j = receiverInfo.EnabledPrns.IndexOf(sat1); //模糊度参数的索引
k = receiverInfo.EnabledPrns.IndexOf(sat2); //模糊度参数的索引
v[i] = NC[i] - (adjustment.Estimated.OneDimArray[j + 5] - adjustment.Estimated.OneDimArray[k + 5]);
// v[time] = NC[time] - (adjustment.Estimated[j + 5] - adjustment.Estimated[k + 5]);
H[j + 5][i] = 1.0;
H[k + 5][i] = -1.0;
R[i][i] = CONST_AMB * CONST_AMB;
}
//update states with constraints
WeightedVector Estimated = filter(adjustment.Estimated.OneDimArray, adjustment.Estimated.InverseWeight.Array, H, v, R, ref factor0);
//set flags
for (i = 0; i < n; i++)
{
sat1.PRN = satN1[i];
sat1.SatelliteType = SatelliteType.G;
sat2.PRN = satN2[i];
sat2.SatelliteType = SatelliteType.G;
ambc[sat1].flags[sat2] = 1;
ambc[sat2].flags[sat1] = 1;
}
return(Estimated);
}
/// <summary>
/// 注册
/// </summary>
/// <param name="time"></param>
/// <param name="ambiguities"></param>
public void Regist(Time time, WeightedVector ambiguities)
{
var rmses = ambiguities.GetRmsVector();
foreach (var item in ambiguities.ParamNames)
{
var sto = this.GetOrCreate(item);
sto[time] = new RmsedNumeral(ambiguities[item], rmses[item]);
}
}
/// <summary>
/// 计算模糊度，部分模糊度固定策略。
/// </summary>
/// <param name="floatSolution">按照Q从小到大的顺序排列后的带权向量。</param>
/// <param name="MaxRatio">最大比例</param>
/// <returns></returns>
public static WeightedVector GetAmbiguity(WeightedVector floatSolution, double MaxRatio = 3, double defaultRms = 1e-20)
{
//integer least square
#region RTKLIB lambda
var currentFloat = floatSolution;
int i = 0;
var lambda = new LambdaAmbiguitySearcher(currentFloat);
double[] N1 = new double[currentFloat.Count * 2]; for (i = 0; i < currentFloat.Count; i++)
{
N1[i] = Math.Round(currentFloat[i]);
}
double[] s = new double[2];
int info = 0;
info = lambda.getLambda(ref N1, ref s);
//if (info == -1 || s[0]<=0.0) return 0; //固定失败
double ratio = s[1] / s[0];
while (info == -1 || ratio <= MaxRatio) //检验没有通过
{
//去掉一个最大的方差值，继续固定。
var nextParamCount = currentFloat.Count - 1;
if (nextParamCount < 1)
{
return(new WeightedVector());
}
currentFloat = currentFloat.GetWeightedVector(0, nextParamCount);
//固定
N1 = new double[currentFloat.Count * 2]; for (i = 0; i < currentFloat.Count; i++)
{
N1[i] = Math.Round(currentFloat[i]);
}
s = new double[2];
lambda = new LambdaAmbiguitySearcher(currentFloat);
info = lambda.getLambda(ref N1, ref s);
if (s[0] == 0 && s[1] != 0) //
{
break;
}
ratio = s[1] / s[0];
}
if (info == -1)
{
return(new WeightedVector());
}
#endregion
var integers = ArrayUtil.GetSubArray <double>(N1, 0, currentFloat.Count);
Vector Vector = new Geo.Algorithm.Vector(integers, currentFloat.ParamNames);
return(new WeightedVector(Vector, defaultRms));
}
/// <summary>
/// 执行无电离层双差模糊度固定
/// </summary>
/// <param name="rawFloatAmbiCycles"></param>
/// <param name="isNetSolve">是否网解</param>
/// <returns></returns>
protected WeightedVector DoFixIonoFreeDoubleDifferAmbiguity(WeightedVector rawFloatAmbiCycles, bool isNetSolve)
{
if (!IsDualIonoFreeComObservation)
{
return(base.DoFixAmbiguity(rawFloatAmbiCycles));
}
//-----------以下为无电离层组合模糊度固定算法--------------------
if (this.DataSourceContext.SiteSatPeriodDataService == null)
{
log.Warn("必须开启时段数据服务，才能实现无电离层模糊度固定！");
return(new WeightedVector());
}
//指定系统的无电离层组合参数计算器
var IonoFreeAmbiguitySolver = IonoFreeAmbiguitySolverManager.GetOrCreate(CurrentBasePrn.SatelliteType);
IonoFreeAmbiguitySolver.CheckOrInit(CurrentBasePrn, CurrentMaterial.ReceiverTime, !Option.IsLengthPhaseValue);
//----------------------第一步 MW 宽巷双差 ------------------------
NameRmsedNumeralVector intMwDoubleDiffers = GetIntMwDoubleDiffers(isNetSolve);
//----------------------第二步 MW 宽巷和模糊度浮点解求窄巷模糊度--------
var ambiFloatVal = rawFloatAmbiCycles.GetNameRmsedNumeralVector();
//求窄巷模糊度浮点解//单位周
var narrowFloat = IonoFreeAmbiguitySolver.GetNarrowFloatValue(intMwDoubleDiffers, ambiFloatVal);
var narrowFloatVect = narrowFloat.GetWeightedVector();
narrowFloatVect.InverseWeight = rawFloatAmbiCycles.GetWeightedVector(narrowFloatVect.ParamNames).InverseWeight; //追加系数阵，按照顺序------
//方法1：
var intNarrowVector = base.DoFixAmbiguity(narrowFloatVect);
var intNarrow = intNarrowVector.GetNameRmsedNumeralVector();// ParseVector(intNarrowVector);
//方法2：直接取整
//var intNarrow = narrowFloatVect.GetRound();//不推荐使用直接取整
//检核窄巷
var intDiffer = intNarrow - narrowFloat;
var toRemoves = intDiffer.GetAbsLargerThan(this.Option.MaxAmbiDifferOfIntAndFloat);
intNarrow.Remove(toRemoves);//移除
//判断是否超限
//计算双差载波模糊度固定值
var fixedVal = IonoFreeAmbiguitySolver.GetIonoFreeAmbiValue(intMwDoubleDiffers, intNarrow);
var result = fixedVal.GetWeightedVector();
return(result);
}
public override WeightedVector GetFixedAmbiguities(WeightedVector FloatAmbiguity)
{
Vector vector = new Vector(FloatAmbiguity.Count);
vector.ParamNames = FloatAmbiguity.ParamNames;
int i = 0;
foreach (var item in FloatAmbiguity)
{
vector[i] = Math.Truncate((Double)item);
i++;
}
return(new WeightedVector(vector, DefaultRms));
}
public IEnumerable <Formation> Generate(Random Random, FormationParameters Parameters)
{
WeightedVector <FormationTemplate> choices = new WeightedVector <FormationTemplate>();
foreach (var template in Subtemplates.Where(i => i.Matches(Parameters)))
{
choices.Add(1 / template.GetExpectedValue(Parameters), template);
}
if (choices.Length > 0)
{
return(choices[Random.NextDouble()].Generate(Random, Parameters));
}
return(Enumerable.Empty <Formation>());
}
/// <summary>
/// 构建表
/// </summary>
/// <param name="fixedParams"></param>
/// <returns></returns>
private static ObjectTableStorage BuildFixedParamTable(WeightedVector fixedParams)
{
ObjectTableStorage result = new ObjectTableStorage("FixedParams");
for (int i = 0; i < fixedParams.Count; i++)
{
result.NewRow();
result.AddItem("Index", i);
result.AddItem("Name", fixedParams.ParamNames[i]);
result.AddItem("TimePeriod", TimePeriod.MaxPeriod);
result.AddItem("Value", fixedParams[i]);
result.AddItem("Rms", 1e-10);
}
return(result);
}
/// <summary>
/// 残差分量检查
/// </summary>
/// <param name="postResiduals"></param>
/// <returns></returns>
public List <string> GetBadElementResidual(WeightedVector postResiduals)
{
List <string> list = new List <string>();
var count = postResiduals.Count;
//for (int i = 0; i < count; i++)
// //纵向比较，统计误差超过5倍的
//{
// var name = postResiduals.ParamNames[i];
// if (String.IsNullOrWhiteSpace(name)) { return list; }//
// var window = ElementResidualCheckers.GetOrCreate(name);
// var val = postResiduals[i];
// var isOk = (window.AverageCheckAddOrClear(product.ReceiverTime, val, MaxErrorTimes));
// if (!isOk )
// {
// list.Add(name);
// }
//}
//横向比较，统计误差，统计误差超过5倍的
var errored = Geo.Utils.DoubleUtil.GetIndexesOfGrossError(postResiduals.GetRmsVector(), MaxErrorTimes, false);
foreach (var index in errored)
{
var name = postResiduals.ParamNames[index];
if (!list.Contains(name))
{
list.Add(name);
}
}
//直接比较残差
errored = Geo.Utils.DoubleUtil.GetIndexesOfGrossError(postResiduals, MaxErrorTimes, false);
foreach (var index in errored)
{
var name = postResiduals.ParamNames[index];
if (!list.Contains(name))
{
list.Add(name);
}
}
return(list);
}
public UnitConfigurationPack(
int Id, string Name, int Number, IEnumerable <UnitConfigurationLock> UnitConfigurationLocks)
{
this.Id = Id;
this.Name = Name;
this.Number = Number;
this.UnitConfigurationLocks = new WeightedVector <UnitConfigurationLock>();
foreach (var ucl in UnitConfigurationLocks)
{
this.UnitConfigurationLocks.Add(ucl.GetWeight(), ucl);
}
ExpectedValue = Number * HarmonicAverage(UnitConfigurationLocks.Select(i => i.GetValue()));
Cost = RoundCost(
.005
* Multiplier(this.UnitConfigurationLocks.Length)
* ExpectedValue,
5);
}
/// <summary>
/// 执行无电离层双差模糊度固定
/// </summary>
/// <param name="rawFloatAmbiCycles"></param>
/// <param name="isNetSolve">是否网解</param>
/// <returns></returns>
protected WeightedVector DoFixIonoFreeDoubleDifferAmbiguity(WeightedVector rawFloatAmbiCycles, NameRmsedNumeralVector intMwDoubleDiffers)
{
IonoFreeAmbiguitySolverManager = new IonoFreeAmbiguitySolverManager();
//指定系统的无电离层组合参数计算器
var IonoFreeAmbiguitySolver = IonoFreeAmbiguitySolverManager.GetOrCreate(CurrentBasePrn.SatelliteType);
IonoFreeAmbiguitySolver.CheckOrInit(CurrentBasePrn, new Time(), true);
//----------------------第一步 MW 宽巷双差 ------------------------
//----------------------第二步 MW 宽巷和模糊度浮点解求窄巷模糊度--------
var ambiFloatVal = rawFloatAmbiCycles.GetNameRmsedNumeralVector();
//求窄巷模糊度浮点解//单位周
var narrowFloat = IonoFreeAmbiguitySolver.GetNarrowFloatValue(intMwDoubleDiffers, ambiFloatVal);
var narrowFloatVect = narrowFloat.GetWeightedVector();
narrowFloatVect.InverseWeight = rawFloatAmbiCycles.GetWeightedVector(narrowFloatVect.ParamNames).InverseWeight; //追加系数阵，按照顺序------
//方法1：
var intNarrowVector = DoFixAmbiguity(narrowFloatVect);
var intNarrow = intNarrowVector.GetNameRmsedNumeralVector();// ParseVector(intNarrowVector);
//方法2：直接取整
//var intNarrow = narrowFloatVect.GetNameRmsedNumeralVector().GetRound();//不推荐使用直接取整
//检核窄巷
var intDiffer = intNarrow - narrowFloat;
var toRemoves = intDiffer.GetAbsLargerThan(this.MaxAmbiDifferOfIntAndFloat);
intNarrow.Remove(toRemoves);//移除
if (toRemoves.Count > 0)
{
log.Info("窄巷移除了 " + Geo.Utils.StringUtil.ToString(toRemoves));
}
//判断是否超限
//计算双差载波模糊度固定值
var fixedVal = IonoFreeAmbiguitySolver.GetIonoFreeAmbiValue(intMwDoubleDiffers, intNarrow);
var result = fixedVal.GetWeightedVector();
return(result);
}
/// <summary>
/// 构建消除固定参数后的新方程。
/// </summary>
/// <param name="input"></param>
/// <returns></returns>
public MatrixEquation Build(MatrixEquation input)
{
var oldParams = input.ParamNames;
var oldCoeff = input.LeftSide;
var oldObs = input.RightSide;
var fixedParams = GetFixedParams(oldParams);
var fixeParamList = fixedParams.Keys.ToList();
Vector fixedVector = new Vector(fixedParams);
//提取新阵
List <string> newParams = Geo.Utils.ListUtil.GetExcept <string>(oldParams, fixeParamList);
SparseMatrix newCoef = BuildNewCoeff(oldCoeff, newParams);
SparseMatrix fixedParamCoef = BuildNewCoeff(oldCoeff, fixeParamList);
Vector fixedObs = new Matrix(fixedParamCoef) * fixedVector;
Vector newObs = oldObs.Col(0) - fixedObs;
WeightedVector newRight = new WeightedVector(newObs, input.QofU);
MatrixEquation result = new MatrixEquation(newCoef, newRight);
return(result);
}
/// <summary>
/// 获取模糊度参数估计结果。单位为周，权逆阵单位依然为米。权逆阵单位改为周，edited by CuiYang.
/// 注意：这里是针对双频的L1和L2。
/// </summary>
/// <returns></returns>
public WeightedVector GetWeightedAmbiguityVectorInCycle()
{
this.ResultMatrix.Estimated.ParamNames = this.ResultMatrix.ParamNames;
//Lamdba方法计算模糊度
List <string> ambiParamNames = new List <string>();
foreach (var name in ResultMatrix.ParamNames)
{
if (name.Contains(Gnsser.ParamNames.DoubleDifferL1Ambiguity) || name.Contains(Gnsser.ParamNames.DoubleDifferL2Ambiguity))
{
ambiParamNames.Add(name);
}
}
WeightedVector estVector = this.ResultMatrix.Estimated.GetWeightedVector(ambiParamNames);
IVector vector = estVector;//系数矩阵是波长，所以，已经是周为单位了。
IMatrix matrix = estVector.InverseWeight;
var vec = new WeightedVector(vector, matrix);
vec.ParamNames = new List <string>(estVector.ParamNames);
return(vec);
}
/// <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>
/// 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);
}
/// <summary>
/// 逐历元进行双差模糊度固定和更新
/// 目前只考虑算GPS的
/// </summary>
/// <param name="rovReceiverInfo"></param>
/// <param name="refReceeiverInfo"></param>
/// <param name="adjustment"></param>
/// <param name="currentBasePrn"></param>
/// <returns></returns>
public WeightedVector Process(EpochInformation rovReceiverInfo, EpochInformation refReceeiverInfo, AdjustResultMatrix adjustment, SatelliteNumber currentBasePrn)
{
double Interval = rovReceiverInfo.ObsInfo.Interval;
if (Interval == 0)
{
MinArcGap = 31.1;
}
else
{
MinArcGap = Interval + 1.1;
}
this.CurrentBasePrn = currentBasePrn;
//如何模糊度子集维数小于4，则该历元所有模糊度采用实数解，参考论文
//如果基准星发生变化，用于存储固定模糊度的字典应全部清空
if (currentBasePrn != LastBasePrn || rovReceiverInfo[currentBasePrn].IsUnstable == true || refReceeiverInfo[currentBasePrn].IsUnstable == true)
{
DoubleDifferWideLaneAmbiCyclesDic = new SortedDictionary <string, DDWideLineAmbiCycles>();
FixIonFreeAmbiCyclesInfo = new Vector();
LastBasePrn = currentBasePrn;
}
//存储基准星
LastBasePrn = currentBasePrn;
WeightedVector fixIonFreeAmbiCycles = new WeightedVector();
if (FixIonFreeAmbiCyclesInfo.Count > 0) //对曾经已经固定过的双差模糊度，如果当前双差卫星状态没有发生变化，则直接传递过来。
{
for (int i = 0; i < rovReceiverInfo.EnabledPrns.Count; i++)
{
SatelliteNumber item = rovReceiverInfo.EnabledPrns[i];
if (item != currentBasePrn)
{
string paramName = item.ToString() + "-" + CurrentBasePrn + Gnsser.ParamNames.PhaseLengthSuffix;
if (FixIonFreeAmbiCyclesInfo.ParamNames.Contains(paramName))
{
if (rovReceiverInfo[item].IsUnstable == true || rovReceiverInfo[currentBasePrn].IsUnstable == true ||
refReceeiverInfo[item].IsUnstable == true || refReceeiverInfo[currentBasePrn].IsUnstable == true)
{
continue;
}
else
{
int index = FixIonFreeAmbiCyclesInfo.ParamNames.IndexOf(paramName);
fixIonFreeAmbiCycles.Add(FixIonFreeAmbiCyclesInfo.Data[index], FixIonFreeAmbiCyclesInfo.ParamNames[index]);
}
}
}
}
}
if (!rovReceiverInfo.SatelliteTypes.Contains(SatelliteType.G) || !refReceeiverInfo.SatelliteTypes.Contains(SatelliteType.G))
{
FixIonFreeAmbiCyclesInfo = fixIonFreeAmbiCycles;
if (fixIonFreeAmbiCycles.Count > minFixedAmbiCount)
{
return(fixIonFreeAmbiCycles);
}
else
{
return(null);
}
}
Vector fixedIntWideLineAmbiCycles = null;
if (!tryFixWideLaneAmbguity(rovReceiverInfo, refReceeiverInfo, out fixedIntWideLineAmbiCycles))
{
//宽巷固定失败
FixIonFreeAmbiCyclesInfo = fixIonFreeAmbiCycles;
if (fixIonFreeAmbiCycles.Count > minFixedAmbiCount)
{
return(fixIonFreeAmbiCycles);
}
else
{
return(null);
}
}
//如何模糊度子集维数小于4，则该历元所有模糊度采用实数解
if (fixedIntWideLineAmbiCycles.Count < minFixedAmbiCount)//1
{
FixIonFreeAmbiCyclesInfo = fixIonFreeAmbiCycles;
if (fixIonFreeAmbiCycles.Count > minFixedAmbiCount)
{
return(fixIonFreeAmbiCycles);
}
else
{
return(null);
}
}
//fix narrow-lane ambiguity
//AR mode: PPP_AR
// stat = fxi_amb_NL_ROUND(rovReceiverInfo, Adjustment, sat1, sat2, NW, m);
//AR mode: PPP_AR ILS
if (!tryFixNarrowLaneAmbiguityByLAMBDA(rovReceiverInfo, refReceeiverInfo, adjustment, fixedIntWideLineAmbiCycles, ref fixIonFreeAmbiCycles))
{
//窄巷固定失败
FixIonFreeAmbiCyclesInfo = fixIonFreeAmbiCycles;
if (fixIonFreeAmbiCycles.Count > minFixedAmbiCount)
{
return(fixIonFreeAmbiCycles);
}
else
{
return(null);
}
}
//存储已经固定成功的模糊度
if (fixIonFreeAmbiCycles.Count > FixIonFreeAmbiCyclesInfo.Count)
{
string ssss = "hahahahaha";
}
FixIonFreeAmbiCyclesInfo = fixIonFreeAmbiCycles;
//for (int i = 0; i < fixIonFreeAmbiCycles.Count; i++)
//{
// var newParam = fixIonFreeAmbiCycles.ParamNames[i];
// var newFixedValue = fixIonFreeAmbiCycles.Data[i];
// if (!FixIonFreeAmbiCyclesInfo.ParamNames.Contains(newParam))
// { FixIonFreeAmbiCyclesInfo.Add(newFixedValue, newParam); }
// else
// {
// int index = FixIonFreeAmbiCyclesInfo.ParamNames.IndexOf(newParam);
// double oldFixedValue = FixIonFreeAmbiCyclesInfo.Data[index];
// if (oldFixedValue != newFixedValue)
// {
// FixIonFreeAmbiCyclesInfo[index] = newFixedValue;
// }
// }
//}
if (fixIonFreeAmbiCycles.Count > minFixedAmbiCount)
{
return(fixIonFreeAmbiCycles);
}
else
{
return(null);
}
}
/// <summary>
/// 默认采用Lambda算法直接固定。
/// 如果是无电离层组合，则需要分别对待，不能直接固定，需要子类进行实现，//2018.11.06，czs， hmx
/// </summary>
/// <param name="rawFloatAmbiCycles"></param>
/// <returns></returns>
protected override WeightedVector DoFixAmbiguity(WeightedVector rawFloatAmbiCycles)
{
return(DoFixIonoFreeDoubleDifferAmbiguity(rawFloatAmbiCycles, true));
}
/// <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>
/// 执行无电离层双差模糊度固定
/// </summary>
/// <param name="rawFloatAmbiCycles"></param>
/// <param name="isNetSolve">是否网解</param>
/// <returns></returns>
protected WeightedVector DoFixIonoFreeAmbiguity(WeightedVector rawFloatAmbiCycles, bool isNetSolve)
{
//-----------以下为无电离层组合模糊度固定算法--------------------
if (this.DataSourceContext.SiteSatPeriodDataService == null)
{
log.Warn("必须开启时段数据服务，才能实现无电离层模糊度固定！");
return(new WeightedVector());
}
var time = this.CurrentMaterial.ReceiverTime;
//指定系统的无电离层组合参数计算器
var IonoFreeAmbiguitySolver = IonoFreeAmbiguitySolverManager.GetOrCreate(CurrentBasePrn.SatelliteType);
IonoFreeAmbiguitySolver.CheckOrInit(CurrentBasePrn, CurrentMaterial.ReceiverTime, !Option.IsLengthPhaseValue);
//----------------------第一步 MW 宽巷双差 ------------------------
var intMwDoubleDiffers = GetIntMwDiffersBeweenSat(isNetSolve);
//----------------------第二步 MW 宽巷和模糊度浮点解求窄巷模糊度--------
var ambiFloatVal = rawFloatAmbiCycles.GetNameRmsedNumeralVector();
//求窄巷模糊度浮点解//单位周
var narrowFloat = IonoFreeAmbiguitySolver.GetNarrowFloatValue(intMwDoubleDiffers, ambiFloatVal);
//--------获取小数偏差部分-------
Dictionary <SatelliteNumber, RmsedNumeral> nlFcbOfBsd = null;
if (NarrawLaneFcbService == null)
{
nlFcbOfBsd = FcbDataService.GetNLFcbOfBsd(time, CurrentBasePrn);
}
else
{
nlFcbOfBsd = NarrawLaneFcbService.GetBsdOfNarrowLane(this.CurrentMaterial.EnabledPrns, CurrentBasePrn, time);
}
var narrowIntFloat = ToStringVector(new NameRmsedNumeralVector <SatelliteNumber>(nlFcbOfBsd));
var narrowNearInt = narrowFloat - narrowIntFloat;
var narrowFloatVect = narrowNearInt.GetWeightedVector();
narrowFloatVect.InverseWeight = rawFloatAmbiCycles.GetWeightedVector(narrowFloatVect.ParamNames).InverseWeight; //追加系数阵，按照顺序------
//--------------------------------窄巷模糊度浮点数减去小数部分-----------------------------------------
//方法1：
var intNarrowVector = base.DoFixAmbiguity(narrowFloatVect);
var intNarrow = intNarrowVector.GetNameRmsedNumeralVector();// ParseVector(intNarrowVector);
//方法2：直接取整
//var intNarrow = narrowFloatVect.GetRound();//不推荐使用直接取整
//检核窄巷
var intDiffer = intNarrow - narrowFloat;
var toRemoves = intDiffer.GetAbsLargerThan(this.Option.MaxAmbiDifferOfIntAndFloat);
intNarrow.Remove(toRemoves);//移除
//判断是否超限
//计算双差载波模糊度固定值
var fixedVal = IonoFreeAmbiguitySolver.GetIonoFreeAmbiValue(intMwDoubleDiffers, intNarrow);
var result = fixedVal.GetWeightedVector();
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);
}
/// <summary>
/// LAMBDA算法固定窄巷模糊度
/// 当然，还有其他固定方法，比如取整固定
/// </summary>
/// <param name="rovReceiverInfo"></param>
/// <param name="refReceeiverInfo"></param>
/// <param name="adjustment"></param>
/// <param name="fixedWindLaneAmbiCycles"></param>
/// <param name="fixedIonFreeAmbiCycles"></param>
/// <returns></returns>
private bool tryFixNarrowLaneAmbiguityByLAMBDA(EpochInformation rovReceiverInfo, EpochInformation refReceeiverInfo, AdjustResultMatrix adjustment, Vector fixedWindLaneAmbiCycles, ref WeightedVector fixedIonFreeAmbiCycles)
{
List <string> ambiParamNames = fixedWindLaneAmbiCycles.ParamNames;
WeightedVector estIonFreeAmbiVector = adjustment.Estimated.GetWeightedVector(ambiParamNames);
double f1 = GnssConst.GPS_L1_FREQ;
double f2 = GnssConst.GPS_L2_FREQ;
IVector narrowLaneAmbiguity = (estIonFreeAmbiVector.Multiply((f1 + f2) / f1)).Minus(fixedWindLaneAmbiCycles.Multiply(f2 / (f1 - f2)));
IMatrix narrowLaneAmbiguityCovariance = estIonFreeAmbiVector.InverseWeight.Multiply((f1 + f2) / f1).Multiply((f1 + f2) / f1);
WeightedVector floatNLAmbiCycles = new WeightedVector(narrowLaneAmbiguity, narrowLaneAmbiguityCovariance);
floatNLAmbiCycles.ParamNames = estIonFreeAmbiVector.ParamNames;
//尝试固定模糊度
//按照权逆阵排序，大的在后面，如果失败后，则删除之
var orderedFloatNLAmbiCycles = floatNLAmbiCycles.GetCovaOrdered();
//模糊度直接用lambda算法进行固定，部分模糊度固定策略
double MaxRatioOfLambda = 3.0;
var fixedIntNLAmbiCycles = Gnsser.LambdaAmbiguitySearcher.GetAmbiguity(orderedFloatNLAmbiCycles, MaxRatioOfLambda);
if (fixedIntNLAmbiCycles.Count < 1)
{
return(false);
}
// 关键问题：模糊度固定是否正确？？
//允许整数与浮点数的最大偏差，如 浮点数为 0.1 而整数为 1，则认为失败。
double maxDifferOfIntAndFloatAmbi = 0.35;
List <string> failedParams = new List <string>();
foreach (var name in fixedIntNLAmbiCycles.ParamNames)
{
if (Math.Abs(fixedIntNLAmbiCycles[name] - orderedFloatNLAmbiCycles[name]) > maxDifferOfIntAndFloatAmbi)
{
double tmp = Math.Abs(fixedIntNLAmbiCycles[name] - orderedFloatNLAmbiCycles[name]);
failedParams.Add(name);
}
}
fixedIntNLAmbiCycles.Remove(failedParams);
if (fixedIntNLAmbiCycles.Count < 1)
{
return(false);
}
//根据固定的宽巷和窄巷模糊度确定无电离层模糊度
foreach (var name in fixedIntNLAmbiCycles.ParamNames)
{
double fixedIntIonFree = fixedIntNLAmbiCycles[name] * f1 / (f1 + f2) + fixedWindLaneAmbiCycles[name] * f1 * f2 / ((f1 - f2) * (f1 + f2));
if (!fixedIonFreeAmbiCycles.ParamNames.Contains(name))
{
fixedIonFreeAmbiCycles.Add(fixedIntIonFree, name);
}
else
{
int index = fixedIonFreeAmbiCycles.ParamNames.IndexOf(name);
double oldFixedValue = fixedIonFreeAmbiCycles.Data[index];
if (oldFixedValue != fixedIntIonFree)
{
fixedIonFreeAmbiCycles[index] = fixedIntIonFree;
}
}
}
return(true);
}
/// <summary>
/// 注册
/// </summary>
/// <param name="time"></param>
/// <param name="ambiguities"></param>
public void Regist(Time time, WeightedVector ambiguities)
{
AmbiguityStorage.Regist(time, ambiguities);
}
private int fxi_amb_NL_ROUND(EpochInformation receiverInfo, AdjustResultMatrix adjustment, int[] satN1, int[] satN2, int[] NW, int n)
{
if (n <= 0)
{
return(0);
}
double C1, C2, B1, v1, BC, v, vc;
double[] NC, var;
double lam_NL = lam_LC(1, 1, 0);
double lam1, lam2;
int i, j, k, m = 0, N1, stat;
lam1 = receiverInfo.First.FrequenceA.Frequence.WaveLength;
lam2 = receiverInfo.First.FrequenceB.Frequence.WaveLength;
C1 = (lam2 * lam2) / (lam2 * lam2 - lam1 * lam1);
C2 = -(lam1 * lam1) / (lam2 * lam2 - lam1 * lam1);
NC = new double[n]; var = new double[n];
Vector vector = new Vector(adjustment.Estimated.OneDimArray);
double[][] P = adjustment.Estimated.InverseWeight.Array;
//for (int time = 5; time < length; time++)
//{
// SatelliteNumber satelliteType = receiverInfo[time - 5].Prn;
// double val = vector[time];
// AmbiguityDic.Add(satelliteType, val);
//}
for (i = 0; i < n; i++)
{
SatelliteNumber sat1 = new SatelliteNumber();
SatelliteNumber sat2 = new SatelliteNumber();
sat1.PRN = satN1[i];
sat1.SatelliteType = SatelliteType.G;
sat2.PRN = satN2[i];
sat2.SatelliteType = SatelliteType.G;
j = receiverInfo.EnabledPrns.IndexOf(sat1); //模糊度参数的索引
k = receiverInfo.EnabledPrns.IndexOf(sat2); //模糊度参数的索引
//narrow-lane ambiguity
B1 = (vector[j + 5] - vector[k + 5] + C2 * lam2 * NW[i]) / lam_NL;
N1 = (int)Math.Floor(B1 + 0.5);
double N10 = Math.Round(B1);
if (N10 != N1)
{
//
}
//variance of narrow-lane ambiguity
var[m] = P[j + 5][j + 5] + P[k + 5][k + 5] - 2.0 * P[k + 5][j + 5];
v1 = var[m] / (lam_NL * lam_NL);
//validation of narrow-lane ambiguity
if (Math.Abs(N1 - B1) > thresar2 || conffunc(N1, B1, Math.Sqrt(v1)) < thresar1)
{
continue;
}
//iono-free ambiguity (m) 固定后的无电离层模糊度，单位 米
BC = C1 * lam1 * N1 + C2 * lam2 * (N1 - NW[i]);
//check residuals
v = adjustment.PostfitResidual[receiverInfo.EnabledSatCount + j] - adjustment.PostfitResidual[receiverInfo.EnabledSatCount + k];
//residuals of carrier-phase (m)???
double v0 = adjustment.ObsMatrix.Observation[receiverInfo.EnabledSatCount + j] - adjustment.ObsMatrix.Observation[receiverInfo.EnabledSatCount + k];
if (v0 != v)
{
}
vc = v + (BC - (vector[j + 5] - vector[k + 5]));
if (Math.Abs(vc) > THRES_RES)
{
continue;
}
satN1[m] = satN1[i];
satN2[m] = satN2[i];
NC[m] = BC;
m++;
}
//select fixed ambiguities by dependancy check
m = sel_amb(satN1, satN2, NC, var, m);
if (m >= 3)
{
double factor0 = 0.0;
//fixed solution
WeightedVector Estimated = fix_sol(receiverInfo, adjustment, satN1, satN2, NC, m, ref factor0);
double factor = adjustment.VarianceOfUnitWeight;
if (factor0 < factor)
{
}
else
{
}
int nx = adjustment.Estimated.Count; //参数个数 n*1
//set solution
for (i = 0; i < nx; i++)
{
adjustment.Estimated[i] = Estimated[i];
for (j = 0; j < nx; j++)
{
adjustment.Estimated.InverseWeight[i, j] = adjustment.Estimated.InverseWeight[j, i] = Estimated.InverseWeight[i, j];
}
}
return(1);
}
else
{
return(0);
}
}
/// <summary>
/// 计算，获取固定后的模糊度结果。
/// </summary>
/// <returns></returns>
public WeightedVector GetFixedAmbiguities(WeightedVector floatResolution)
{
this.Dimension = floatResolution.Count;
this.Cova = floatResolution.InverseWeight;
this.FloatResolution = floatResolution;
Init();
int MaxCan = 2;
ArrayMatrix ZTransformation = ArrayMatrix.Diagonal(Dimension, Dimension, 1); // initialize Z to a unit matrix
//make the L_1^{-TProduct} D_1^{-1} L_1^{-1} decomposition of the covariance matrix Q
//分解协方差阵为 D L
double[] D = new double[Dimension];
ArrayMatrix L = new ArrayMatrix(Dimension, Dimension);
FactorizationQToLTDL(Cova, ref D, ref L);
//计算 Z 转换矩阵，采用 D 和 L
Re_Order(ZTransformation, D, L);
ArrayMatrix Zt = ZTransformation.Inverse; //inversion yields transpose of Z
//For the search we need L and D of Q^{-1}
L = GetInverseOfLowerTriangular(L, Dimension);
for (int i = 0; i < Dimension; i++)
{
D[i] = 1.0 / D[i];
}
//find a suitable Chi^2 such that we have dayServices candidates an minimum:
//use an eps to make sure the dayServices candidates are inside the ellipsoid some small number
double eps = Math.Pow(10, -6);
double[] v3 = Chistart(D, L);
double Chi_1 = v3[1] + eps;
//find the dayServices candidates with minimun norm
ArrayMatrix candidates = new ArrayMatrix(Dimension, MaxCan); // 2-dimensional array to store the candidates
ArrayMatrix disal1 = new ArrayMatrix(1, MaxCan); //according squared norms
int ipos = 0; //最佳候选整数的位置
int ncan = 0;
Search(Chi_1, MaxCan, Dimension, D, L, Fractions, ref disal1, ref candidates, ref ipos, ref ncan);
//back-transformation (and adding increments)
//还原为Y 的整数估计^Y
Vector Results = new Vector(Dimension);
for (int i = 0; i < Dimension; i++)
{
for (int j = 0; j < Dimension; j++)
{
Results[i] += ZTransformation[i, j] * candidates[j, ipos];
}
Results[i] = Results[i] + RawIntegers[i];
}
//'sort' the squared norms in increasing order (if ipos equals 1, the best candidate is in the second place: so reverse disal1)
if (ipos == 1)
{
double h = disal1[0, 0];
disal1[0, 0] = disal1[0, 1];
disal1[0, 1] = h;
}
return(new WeightedVector(Results, DefaultRms));
}
/// <summary>
/// update states with constraints
/// </summary>
/// <param name="x">states vector (n * 1)</param>
/// <param name="P">covariance matrix of states (n * n)</param>
/// <param name="H">transpose of design matrix (n*m)</param>
/// <param name="v">innovation (measurement -model ) (m *1 )</param>
/// <param name="R">covariance matrix of measurement error (m * m)</param>
/// <returns></returns>
private WeightedVector filter(double[] x, double[][] P, double[][] H, double[] v, double[][] R, ref double factor0)
{
Vector tx = new Vector(x);
Vector tv = new Vector(v);
IMatrix P_ = new ArrayMatrix(P);
IMatrix H_ = new ArrayMatrix(H);
IMatrix R_ = new ArrayMatrix(R);
IMatrix x_ = new ArrayMatrix(tx);
IMatrix v_ = new ArrayMatrix(tv);
IMatrix H_T = H_.Transposition;
//IMatrix P_o = R_.GetInverse();
//IMatrix P1 = P_.GetInverse();
//IMatrix invTemp = (H_.Multiply(P_o).Multiply(H_T)).Plus(P1);
//// Compute the a posteriori error covariance matrix
//IMatrix CovaOfEstParam = invTemp.GetInverse();
//IMatrix EstParam = CovaOfEstParam.Multiply(H_.Multiply(P_o).Multiply(v_).Plus(P1.Multiply(x_)));
IMatrix QL = R_.Plus(H_T.Multiply(P_).Multiply(H_));
IMatrix J = (P_.Multiply(H_)).Multiply(QL.GetInverse());
IMatrix dX = J.Multiply(v_);
IMatrix QdX = J.Multiply(H_T).Multiply(P_);
IMatrix newX = x_.Minus(dX); // CovaOfEstParam.Multiply(AT.Multiply(P_o).Multiply(observation).Plus(P1.Multiply(PredictParam)));
IMatrix newP = P_.Minus(QdX);
IMatrix V = H_T.Multiply(dX).Minus(v_);
IMatrix VTPV = V.Transposition.Multiply(R_.GetInverse()).Multiply(V).Plus((dX.Transposition).Multiply(P_.GetInverse()).Multiply(dX));
//double det02 = VTPV[0, 0] / control.RowCount;
//double det0 = Math.Sqrt(det02);
factor0 = VTPV[0, 0] / H_T.RowCount;
//IMatrix AT = control.Transposition;
//IMatrix P_o = covaOfModelNoice.GetInverse();
//IMatrix P1 = CovaOfPredictParam.GetInverse();
//IMatrix invTemp = AT.Multiply(P_o).Multiply(control).Plus(P1);
//// Compute the a posteriori error covariance matrix
//IMatrix CovaOfEstParam = invTemp.GetInverse();
//IMatrix EstParam = CovaOfEstParam.Multiply(AT.Multiply(P_o).Multiply(observation).Plus(P1.Multiply(PredictParam)));
IMatrix Tmp = (H_T.Multiply(P_)).Multiply(H_).Plus(R_);
IMatrix K = P_.Multiply(H_).Multiply(Tmp.GetInverse());
IMatrix xp = x_.Plus(K.Multiply(v_)); // CovaOfEstParam.Multiply(AT.Multiply(P_o).Multiply(observation).Plus(P1.Multiply(PredictParam)));
IMatrix Pp = P_.Minus(K.Multiply(H_T).Multiply(P_));
WeightedVector Estimated = new WeightedVector(xp, Pp);
WeightedVector Estimated0 = new WeightedVector(newX, newP);
// return Estimated; //OK
return(Estimated0); //OK
}
/// <summary> /// 固定算法实现 /// </summary> /// <param name="floatResolution"></param> /// <returns></returns> public abstract WeightedVector GetFixedAmbiguities(WeightedVector floatResolution);
