/// <summary>
/// 如果有效，则直接返回，如果无效则重置先验值重新计算。
/// </summary>
/// <param name="epochInfo"></param>
/// <param name="result"></param>
/// <returns></returns>
private SingleSiteGnssResult CheckOrRecaculate(EpochInformation epochInfo, SingleSiteGnssResult result)
{
if (Geo.Algorithm.Vector.IsValid(result.ResultMatrix.StdOfEstimatedParam))
{
return(result);
}
var lastOkIndex = PrevResults.LastKey;
var differIndex = CurrentIndex - lastOkIndex;
var newAprioriIndex = PrevResults.LastKey - differIndex;
while (newAprioriIndex > 0)
{
if (PrevResults.Contains(newAprioriIndex))
{
var product = PrevResults[newAprioriIndex];
this.SetProduct(product);
if (differIndex > 5)
{
//this.MatrixBuilder.IsResetCovaOfApriori = true;
}
break;
}
newAprioriIndex--;
}
return(base.CaculateKalmanFilter(epochInfo, this.CurrentProduct));
}
/// <summary>
/// 单点定位结果转化为Sinex文件
/// </summary>
/// <returns></returns>
public static SinexFile Build(SingleSiteGnssResult result)
{
SinexFile sinex = new SinexFile("Gnsser");
sinex.SolutionEpochBlock.Items.Add(new SolutionEpoch()
{
DateStart = result.ReceiverTime,
DateEnd = result.ReceiverTime,
DateMean = result.ReceiverTime,
ObservationCode = "P",
PointCode = "A",
SiteCode = result.SiteInfo.SiteName,
SolutionID = "0001"
});
SinexStatistic stat = new SinexStatistic()
{
NumberOfUnknown = result.ResultMatrix.ParamCount,
NumberOfObservations = result.ResultMatrix.ObsMatrix.Observation.Count,
VarianceOfUnitWeight = result.ResultMatrix.VarianceOfUnitWeight,
NumberOfDegreesOfFreedom = result.ResultMatrix.Freedom
};
sinex.SolutionStattisticsBlock.Items = stat.GetSolutionStatistics();
GeoCoord approxGeo = CoordTransformer.XyzToGeoCoord(result.ApproxXyz);
sinex.SiteIdBlock.Items.Add(new SiteId()
{
SiteCode = result.SiteInfo.SiteName,
PointCode = "A",
UniqueMonumentId = result.SiteInfo.MarkerNumber,
ApproximateHeight = approxGeo.Height,
ApproximateLatitude = approxGeo.Lat,
ApproximateLongitude = approxGeo.Lon,
ObservationCode = "P",
GeoCoord = result.GeoCoord,
StationDescription = "Single Point"
});
int index = 0;
sinex.SolutionEstimateBlock.Items.AddRange(new SolutionValue[] {
new SolutionValue()
{
Index = 1 + index++,
ParameterType = "STAX",
ParameterValue = result.EstimatedXyz.X,
SiteCode = result.SiteInfo.SiteName,
RefEpoch = result.ReceiverTime,
PointCode = "A",
ParameterUnits = "m",
ConstraintCode = "2",
StdDev = Math.Sqrt(result.ResultMatrix.CovaOfEstimatedParam[index - 1, index - 1]),
SolutionID = "0001"
}, new SolutionValue()
{
Index = 1 + index++,
ParameterType = "STAY",
ParameterValue = result.EstimatedXyz.Y,
SiteCode = result.SiteInfo.SiteName,
RefEpoch = result.ReceiverTime,
PointCode = "A",
ParameterUnits = "m",
ConstraintCode = "2",
StdDev = Math.Sqrt(result.ResultMatrix.CovaOfEstimatedParam[index - 1, index - 1]),
SolutionID = "0001"
}, new SolutionValue()
{
Index = 1 + index++,
ParameterType = "STAZ",
ParameterValue = result.EstimatedXyz.Z,
SiteCode = result.SiteInfo.SiteName,
RefEpoch = result.ReceiverTime,
PointCode = "A",
ParameterUnits = "m",
ConstraintCode = "2",
StdDev = Math.Sqrt(result.ResultMatrix.CovaOfEstimatedParam[index - 1, index - 1]),
SolutionID = "0001"
},
new SolutionValue()
{
Index = 1 + index++,
ParameterType = "RBIAS",
ParameterValue = result.RcvClkErrDistance,
SiteCode = result.SiteInfo.SiteName,
RefEpoch = result.ReceiverTime,
PointCode = "A",
ParameterUnits = "m",
ConstraintCode = "2",
StdDev = Math.Sqrt(result.ResultMatrix.CovaOfEstimatedParam[index - 1, index - 1]),
SolutionID = "0001"
}
});
// sinex.
sinex.SolutionMatrixEstimateCova.Items = SinexMatrixConvertor.GetMatrixLines(result.ResultMatrix.CovaOfEstimatedParam.Array);
return(sinex);
}
/// <summary>
/// PPP 计算核心方法。 Kalmam滤波。
/// 观测量的顺序是先伪距观测量，后载波观测量，观测量的总数为卫星数量的两倍。
/// 参数数量为卫星数量加5,卫星数量对应各个模糊度，5为3个位置量xyz，1个接收机钟差量，1个对流程湿分量。
/// </summary>
/// <param name="recInfo">接收信息</param>
/// <param name="option">解算选项</param>
/// <param name="lastPppResult">上次解算结果（用于 Kalman 滤波）,若为null则使用初始值计算</param>
/// <returns></returns>
public override SingleSiteGnssResult CaculateKalmanFilter(EpochInformation recInfo, SingleSiteGnssResult lastPppResult = null)
{
PppResult last = null;
if (lastPppResult != null)
{
last = (PppResult)lastPppResult;
}
// ISatWeightProvider SatWeightProvider = new SatElevateAndRangeWeightProvider();
//ISatWeightProvider SatWeightProvider = new SatElevateWeightProvider();
MatrixBuilder.SetMaterial(recInfo).SetPreviousProduct(lastPppResult);
MatrixBuilder.Build();
if (this.MatrixBuilder.ObsCount > 0)
{
// this.Adjustment = new KalmanFilter( this.MatrixBuilder);
this.Adjustment = this.RunAdjuster(BuildAdjustObsMatrix(this.CurrentMaterial));
////尝试固定模糊度 cuiyang 2015.07
//int fixFlag = Ppp_AR.Process(recInfo, Adjustment);
#region 具有条件的参数平差
// this.Adjustment.restrictadjust();
#endregion
#region PPP模糊度固定
foreach (var sat in recInfo.EnabledPrns)
{
if (recInfo.SlipedPrns.Contains(sat)) //主要是针对 1.首次观测到某颗卫星 2.某颗卫星出现在第二个弧段，两个弧段的MW不相等
{
if (MwInfoDic.ContainsKey(sat))
{
MwInfoDic.Remove(sat);
}
continue;
}
if (!MwInfoDic.ContainsKey(sat)) //第一个没有周跳的历元
{
MwInfoDic[sat] = new SmoothValue(100, true)
{
Name = sat.ToString()
};
}
MwInfoDic[sat].Regist(recInfo[sat].Combinations.MwRangeCombination.Value);
}
List <int> cyclesat = new List <int>(); //将有周跳的卫星挑出来
foreach (var sat in recInfo.SlipedPrns)
{
cyclesat.Add(sat.PRN);
}
if (MwInfoDic.Count > 0)
{
Dictionary <int, double> MWs = new Dictionary <int, double>();
foreach (var sat in recInfo.EnabledPrns)
{
if (!recInfo.SlipedPrns.Contains(sat))
{
int num = sat.PRN;
MWs.Add(num, MwInfoDic[sat].Value); //宽巷模糊度的历元平滑值
}
}
IMatrix new_estimated = this.Adjustment.PppArmbiResolve(MWs, cyclesat);
}
#endregion
PppResult result = new PppResult(recInfo, Adjustment, this.MatrixBuilder.GnssParamNameBuilder);
// result.PreviousResult = lastPppResult;
//模糊度设置
IonoFreeAmbiguityMgr.SetIonoFreeCombination(result);
return(result);
}
else
{
return(null);
}
}
/// <summary>
/// 快速计算一个坐标。自动重置数据流。
/// </summary>
/// <param name="DataSourceContext"></param>
/// <returns></returns>
public static SingleSiteGnssResult GetApproxPosition(DataSourceContext DataSourceContext)
{
GnssProcessOption PositionOption = GnssProcessOption.GetDefaultSimplePseudoRangePositioningOption();
var p = new SimpleRangePositioner(DataSourceContext, PositionOption)
{
IsUpdateEstimatePostition = true,
IsPostCheckEnabled = false,//取消检查，否则报错，2019.05.16, czs
};
var Reviser = EpochInfoReviseManager.GetDefaultEpochInfoReviser(DataSourceContext, PositionOption, null);
var checker = EpochCheckingManager.GetDefaultCheckers(DataSourceContext, PositionOption);
var EpochEphemerisSetter = new EpochEphemerisSetter(DataSourceContext, PositionOption);
lock (locker)
{
var initMaxRms = PositionOption.MaxStdDev;
var initType = PositionOption.CaculateType;
var initUpdate = PositionOption.IsUpdateEstimatePostition;
var initPostCheck = PositionOption.IsResidualCheckEnabled;
var initResultCheck = PositionOption.IsResultCheckEnabled;
PositionOption.CaculateType = CaculateType.Independent;
PositionOption.MaxStdDev = 9999999;
PositionOption.IsResidualCheckEnabled = false;
PositionOption.IsResultCheckEnabled = false;
PositionOption.IsUpdateEstimatePostition = initUpdate;
int i = 0;
SingleSiteGnssResult GnssResult = null;
List <SingleSiteGnssResult> results = new List <SingleSiteGnssResult>();
foreach (var item in DataSourceContext.ObservationDataSource)
{
i++;
if (!checker.Check(item))
{
continue;
}
var info = item;
// if (!Reviser.Revise(ref info)) { continue; }
Reviser.Revise(ref info);
if (info.EnabledSatCount < 4)
{
continue;
}
GnssResult = p.Get(item);
if (GnssResult != null)
{
item.SiteInfo.SetApproxXyz(item.SiteInfo.EstimatedXyz);
results.Add(GnssResult);
}
if (i > 5 && results.Count > 0)
{
results.Sort(new Comparison <SingleSiteGnssResult>(delegate(SingleSiteGnssResult a, SingleSiteGnssResult b) { return((int)(1000 * (a.EstimatedXyzRms.Length - b.EstimatedXyzRms.Length))); }));
GnssResult = results[0];
break;
}
}
PositionOption.IsResidualCheckEnabled = initPostCheck;
PositionOption.IsUpdateEstimatePostition = initUpdate;
PositionOption.IsResultCheckEnabled = initResultCheck;
PositionOption.CaculateType = initType;
DataSourceContext.ObservationDataSource.Reset();
PositionOption.MaxStdDev = initMaxRms;
return(GnssResult);
}
}
/// <summary>
/// 滤波计算
/// </summary>
/// <param name="epochInfo"></param>
/// <param name="lastResult"></param>
/// <returns></returns>
public override SingleSiteGnssResult CaculateKalmanFilter(EpochInformation epochInfo, SingleSiteGnssResult lastResult)
{
//极速模式。
epochInfo.RemoveIonoFreeUnavailable();
if (epochInfo.Count < 2)
{
log.Error("卫星可用数量不足：" + epochInfo.Count);
return(null);
}
var result = base.CaculateKalmanFilter(epochInfo, lastResult) as PppResult;
//外部模糊度文件直接固定
if (Option.IsFixingAmbiguity && Option.IsUseFixedParamDirectly && File.Exists(Option.AmbiguityFilePath) && Option.IsUsingAmbiguityFile)
{
return(result);
}
return(result);
}
public override SingleSiteGnssResult CaculateKalmanFilter(EpochInformation epochInfo, SingleSiteGnssResult lastResult)
{
CurrentIndex++;
var result = base.CaculateKalmanFilter(epochInfo, lastResult);
if (CurrentProduct != null)
{
if (XYZ.IsValueValid(CurrentProduct.EstimatedXyzRms))
{
LastValidXyzRms = CurrentProduct.EstimatedXyzRms;
}
}
result = CheckOrRecaculate(epochInfo, result);
if (!XYZ.IsValueValid(result.EstimatedXyzRms))
{
log.Error("RMS 检核未通过 " + result.EstimatedXyzRms);
return(null);
}
//if (LastValidXyzRms != null
// && CurrentIndex > 10 //先让其浪一会儿
// && (result.EstimatedXyzRms.Length > LastValidXyzRms.Length * 50))
//{
// log.Error("RMS 检核未通过 " + result.EstimatedXyzRms);
// return null;
//}
//ok
PrevResults.Add(CurrentIndex, result);
return(result);
}
/// <summary>
/// 滤波计算
/// </summary>
/// <param name="epochInfo"></param>
/// <param name="lastResult"></param>
/// <returns></returns>
public override SingleSiteGnssResult CaculateKalmanFilter(EpochInformation epochInfo, SingleSiteGnssResult lastResult)
{
if (RangePositioner != null)
{
var posResult = RangePositioner.CurrentProduct;
}
var result = base.CaculateKalmanFilter(epochInfo, lastResult) as PppResult;
#region
if (this.IsFixingAmbiguity && epochInfo.Time.Value.SecondsOfDay > epochInfo.ObsInfo.StartTime.SecondsOfDay + 120)//&& this.Adjustment.ParamCount>10000000
{
#region 读取自定义FCB文件
string WLpath = Path.Combine(Setting.DataDirectory, @"GNSS\Attachment\2013001FCB\WLFCB.txt");
Dictionary <SatelliteNumber, double> SDWL_FCB = new Dictionary <SatelliteNumber, double>();
Dictionary <SatelliteNumber, double> SDNL_FCB = new Dictionary <SatelliteNumber, double>();
using (var streamReader = new StreamReader(WLpath))
{
var line = "";
Time time = Time.Default;
while ((line = streamReader.ReadLine()) != null)
{
SatelliteNumber prn = SatelliteNumber.Parse(line.Substring(0, 3));
var val = Double.Parse(line.Substring(4));
SDWL_FCB.Add(prn, val);
}
}
//???NLFCB.txt 文件头注释采用什么星历和钟差估计的FCB
string NLpath = Path.Combine(Setting.DataDirectory, @"GNSS\Attachment\2013001FCB\NLFCB.txt");
using (var streamReader = new StreamReader(NLpath))
{
var line = "";
Time time = Time.Default;
while ((line = streamReader.ReadLine()) != null)
{
SatelliteNumber prn = SatelliteNumber.Parse(line.Substring(0, 3));
var val = Double.Parse(line.Substring(4));
SDNL_FCB.Add(prn, val);
}
}
#endregion
//???此处采用G01，应改为通用
SatelliteNumber MaxPrn = SatelliteNumber.Parse("G01");
#region MW值逐历元平滑
List <SatelliteNumber> SatWithCycleSlips = new List <SatelliteNumber>();
//epochInfo.GetCycleSlipedPrns();
Dictionary <SatelliteNumber, double> Smooth_MWs = new Dictionary <SatelliteNumber, double>();
foreach (var sat in epochInfo.TotalPrns)
{
if (SatWithCycleSlips.Contains(sat))//主要是针对 1.首次观测到某颗卫星 2.某颗卫星出现在第二个弧段，两个弧段的MW不相等&& epochInfo.Time.Value.Minute== 21&& epochInfo.Time.Value.Second ==30
{
if (SUM_MWs.ContainsKey(sat))
{
SUM_MWs.Remove(sat);
}
continue;
}
if (SUM_MWs.ContainsKey(sat))//累加
{
//SUM_MWs[sat].sum += epochInfo.MWs[sat];
SUM_MWs[sat].sum += epochInfo[sat].MwCycle;
SUM_MWs[sat].total += 1;
}
else //第一个没有周跳的历元
{
MWInfo tmpmw = new MWInfo();
//tmpmw.sum = epochInfo.MWs[sat];
tmpmw.sum = epochInfo[sat].MwCycle;
tmpmw.total = 1;
SUM_MWs.Add(sat, tmpmw);
}
}
//当前历元没有出现改课卫星时，要将其从SUM_MWs清除
List <SatelliteNumber> tmpsats = new List <SatelliteNumber>(SUM_MWs.Keys);
foreach (var sat in tmpsats)
{
if (epochInfo.EnabledPrns.Contains(sat))
{
continue;
}
else
{
SUM_MWs.Remove(sat);
}
}
if (SUM_MWs.Count > 0)
{
foreach (var sat in epochInfo.TotalPrns)
{
if (!SatWithCycleSlips.Contains(sat))
{
Smooth_MWs.Add(sat, SUM_MWs[sat].sum / SUM_MWs[sat].total);//宽巷模糊度的历元平滑值
}
}
}
#endregion
#region 读取FCB文件
//string path = "D:\\Data\\GNSS\\19240FCB\\sgg19240_igs_30.fcb";
//FcbFileReader reader = new FcbFileReader(path);
//FcbFile = reader.Read();
////对其到15min的FCB
//double tmp = Math.Floor(epochInfo.Time.Value.SecondsOfDay / 900) * 900 + Math.Floor(epochInfo.Time.Value.SecondsOfWeek / 86400) *86400;
//Time current = new Time(epochInfo.Time.Value.GpsWeek, tmp);
//var data = FcbFile.FcbInfos;
//var currentdata = data.FindAll(m => m.Time == current);
#endregion
#region 所有卫星的星间单差宽巷及窄巷FCB
//选择高度角最大的那颗卫星
//SatelliteNumber MaxPrn;
//MaxPrn = epochInfo[0].Prn;
//foreach (var item in epochInfo)
//{
// if (epochInfo[MaxPrn].Polar.Elevation < item.Polar.Elevation)
// {
// MaxPrn = item.Prn;
// }
// else
// {
// continue;
// }
//}
////及那颗卫星的FCB
//double ReferenceNLFCB = 0.0;
//foreach(var item in currentdata)
//{
// if(item.Prn == MaxPrn)
// {
// ReferenceNLFCB = item.Value;
// }
//}
////星间单差FCB
//Dictionary<SatelliteNumber, double> SDNL_FCB = new Dictionary<SatelliteNumber, double>();
//foreach(var item in currentdata)
//{
// SDNL_FCB.Add(item.Prn, item.Value - ReferenceNLFCB);
//}
//var wl = FcbFile.Header.WideLaneValue;
//double ReferenceWLFCB = 0.0;
//foreach (var item in wl.Data)
//{
// if (item.Key == MaxPrn)
// {
// ReferenceWLFCB = item.Value.Value;
// }
//}
////星间单差宽巷FCB
//Dictionary<SatelliteNumber, double> SDWL_FCB = new Dictionary<SatelliteNumber, double>();
//foreach (var item in wl.Data)
//{
// SDWL_FCB.Add(item.Key, item.Value.Value - ReferenceWLFCB);
//}
#endregion
#region 星间单差宽巷模糊度//星间单差宽巷模糊度,减去参考星,不包括出现周跳的卫星
Dictionary <SatelliteNumber, double> SD_MW = new Dictionary <SatelliteNumber, double>();
foreach (var item in Smooth_MWs)
{
if (item.Key == MaxPrn)
{
continue;
}
SD_MW.Add(item.Key, item.Value - Smooth_MWs[MaxPrn]);
}
#endregion
#region 对MW值（宽巷模糊度）进行宽巷FCB纠正，注意是-,是减
Dictionary <SatelliteNumber, double> correctedSD_MW = new Dictionary <SatelliteNumber, double>();
foreach (var item in SD_MW)
{
correctedSD_MW.Add(item.Key, item.Value - SDWL_FCB[item.Key]); //改正WLFCB
}
Dictionary <SatelliteNumber, double> fixedSD_MW = new Dictionary <SatelliteNumber, double>();
foreach (var item in correctedSD_MW)
{
if ((Math.Abs(correctedSD_MW[item.Key] - Math.Round(correctedSD_MW[item.Key])) <= 0.25)) //NLFCBs=0代表无效
{
fixedSD_MW.Add(item.Key, Math.Round(correctedSD_MW[item.Key]));
}
}
#endregion
#region 对窄巷模糊度进行NLFCB改正,注意是-
Dictionary <SatelliteNumber, double> floatAmb = new Dictionary <SatelliteNumber, double>();//存储无电离层浮点模糊度,有参考星
for (int i = 5; i < this.Adjustment.ParamCount; i++)
{
SatelliteNumber sat = SatelliteNumber.Parse(this.Adjustment.ParamNames[i].Substring(0, 3));
//if (SatWithCycleSlips.Contains(sat)) continue;
floatAmb.Add(sat, this.Adjustment.Estimated[i, 0]); //估值
}
Dictionary <SatelliteNumber, double> SD_floatAmb = new Dictionary <SatelliteNumber, double>(); //存储星间单差无电离层浮点模糊度,删除参考星,单位：周
foreach (var sat in floatAmb.Keys)
{
if (sat == MaxPrn)
{
continue;
}
SD_floatAmb.Add(sat, (floatAmb[sat] - floatAmb[MaxPrn]) / GnssConst.GPS_L1_WAVELENGTH);//
}
Dictionary <SatelliteNumber, double> SDNL_floatAmb = new Dictionary <SatelliteNumber, double>();//存储窄巷浮点模糊度,没有参考星，注意是- ??????????
foreach (var sat in fixedSD_MW.Keys)
{
if (SDNL_FCB[sat] != 0)
{
double tmpnl = SD_floatAmb[sat] * (1575.42 + 1227.60) / 1575.42 - fixedSD_MW[sat] * 1227.60 / (1575.42 - 1227.60) - SDNL_FCB[sat];
SDNL_floatAmb.Add(sat, tmpnl);
}
}
#endregion
#region LAMBDA固定窄巷模糊度
if (SDNL_floatAmb.Count >= 4) //模糊度维度>4的情况才进行固定
{
#region 星间单差窄巷模糊度协方差矩阵
IMatrix B = new ArrayMatrix(this.Adjustment.ParamNames.Count - 6, this.Adjustment.ParamNames.Count - 5, 0.0); //将非差模糊度转换为星间单差模糊度的旋转矩阵
int index = 0; //参考星的索引位置???,参考星不同，index不同
for (int i = 5; i < this.Adjustment.ParamNames.Count; i++)
{
SatelliteNumber sat = SatelliteNumber.Parse(this.Adjustment.ParamNames[i].Substring(0, 3));
if (sat == MaxPrn)
{
break;
}
else
{
index++;
}
}
for (int i = 0; i < this.Adjustment.ParamNames.Count - 6; i++)
{
if (i < index)
{
B[i, index] = -1; B[i, i] = 1;
}
if (i >= index)
{
B[i, index] = -1; B[i, i + 1] = 1;
}
}
IMatrix BT = B.Transposition;
IMatrix covaAmb = new ArrayMatrix(this.Adjustment.ParamNames.Count - 5, this.Adjustment.ParamNames.Count - 5, 0); //非差模糊度的协方差矩阵
for (int i = 0; i < this.Adjustment.ParamNames.Count - 5; i++)
{
for (int j = 0; j < this.Adjustment.ParamNames.Count - 5; j++)
{
covaAmb[i, j] = this.Adjustment.CovaOfEstimatedParam[i + 5, j + 5];
}
}
IMatrix SD_covaAmb = B.Multiply(covaAmb).Multiply(BT); //星间单差模糊度的协方差矩阵
IMatrix SDNL_covaAmb = new ArrayMatrix(this.Adjustment.ParamNames.Count - 6, this.Adjustment.ParamNames.Count - 6, 0.0); //星间单差窄巷模糊度的协方差矩阵
for (int i = 0; i < this.Adjustment.ParamNames.Count - 6; i++)
{
for (int j = 0; j < this.Adjustment.ParamNames.Count - 6; j++)
{
SDNL_covaAmb[i, j] = SD_covaAmb[i, j] * (1575.42 + 1227.60) / (1575.42 * GnssConst.GPS_L1_WAVELENGTH);//系数
}
}
#endregion
#region 部分模糊度固定，先将可以固定的星间单差窄巷模糊度 及其 协方差矩阵挑出来
List <string> old_ambpara = new List <string>();
foreach (var item in SD_floatAmb.Keys)
{
old_ambpara.Add(item.ToString());//所有的模糊度（除了参考星）
}
List <string> new_ambpara = new List <string>();
foreach (var item in SDNL_floatAmb.Keys)
{
new_ambpara.Add(item.ToString()); //可以固定的模糊度（宽巷 < 0.25 && NLFCB != 0）
}
IMatrix newtrix = NamedMatrix.GetNewMatrix(new_ambpara, old_ambpara, SDNL_covaAmb.Array); //将可以固定的模糊度的协方差矩阵挑出来
#endregion
#region LAMBDA星间单差窄巷模糊度固定
IMatrix narrowLaneAmbiguity = new ArrayMatrix(SDNL_floatAmb.Count, 1, 0);
int ii = 0;
foreach (var item in SDNL_floatAmb.Values)
{
narrowLaneAmbiguity[ii, 0] = item;
ii++;
}
#region 降维，但是存在相等就麻烦了
double[] a = narrowLaneAmbiguity.GetCol(0).OneDimArray;
double[][] Q = newtrix.Array;
LlyLambda lambda = new LlyLambda(new_ambpara.Count, 2, Q, a);//LAMBDA算法
double[] N1 = new double[a.Length * 2]; for (int i = 0; i < a.Length; i++)
{
N1[i] = Math.Floor(a[i] + 0.5);
}
double[] s = new double[2];
int info = 0;
info = lambda.getLambda(ref N1, ref s);
double ratio = s[1] / s[0];
#endregion
#endregion
//宽巷模糊度和窄巷模糊度固定后反求出的无电离层组合模糊度
Dictionary <SatelliteNumber, double> fixedIonoAmb = new Dictionary <SatelliteNumber, double>();
#region 求固定后的无电离层模糊度
if (ratio > 3)
{
if (N1.Length == newtrix.RowCount * 2)//没有出现降维
{
List <SatelliteNumber> validsats = new List <SatelliteNumber>(SDNL_floatAmb.Keys);
for (int i = 0; i < newtrix.RowCount; i++)
{
SatelliteNumber sat = validsats[i];
double currentWL = fixedSD_MW[sat]; //该卫星的宽巷,注意是整数
double currentNL = N1[i]; //该卫星的窄巷,注意是整数
// double freqOfN =
double freqOfW = 1575.42 * 1227.60 / (1575.42 * 1575.42 - 1227.6 * 1227.60);
double freqOfN = 1575.42 / (1575.42 + 1227.60);
double currentIonoAmb = freqOfW * currentWL + freqOfN * currentNL + freqOfN * SDNL_FCB[sat];
fixedIonoAmb.Add(sat, currentIonoAmb);
}
}
IMatrix restrictMatrix = new ArrayMatrix(fixedIonoAmb.Count, this.Adjustment.ParamCount, 0); //约束矩阵
IMatrix W = new ArrayMatrix(fixedIonoAmb.Count, 1, 0); //W矩阵
ii = 0;
List <SatelliteNumber> totalsats = new List <SatelliteNumber>(epochInfo.EnabledPrns);
foreach (var sat in fixedIonoAmb.Keys)
{
int currentIndex = totalsats.IndexOf(sat);//获取索引值
restrictMatrix[ii, 5 + index] = -1;
restrictMatrix[ii, 5 + currentIndex] = 1;
W[ii, 0] = (SD_floatAmb[sat] - fixedIonoAmb[sat]) * GnssConst.GPS_L1_WAVELENGTH;
ii++;
}
#region 具有约束条件的参数平差 ???条件平差给不同的权，得到不同的结果
IMatrix A = restrictMatrix;
IMatrix AT = A.Transposition;
IMatrix AQAT = A.Multiply(this.Adjustment.CovaOfEstimatedParam).Multiply(AT);
IMatrix Inv_AQAT = AQAT.GetInverse();
IMatrix tmp1 = this.Adjustment.CovaOfEstimatedParam.Multiply(AT).Multiply(Inv_AQAT);
IMatrix tmp2 = tmp1.Multiply(W);
IMatrix X1 = new ArrayMatrix(this.Adjustment.ParamNames.Count, 1, 0);
for (int i = 0; i < this.Adjustment.ParamNames.Count; i++)
{
X1[i, 0] = this.Adjustment.Estimated[i, 0];
}
IMatrix new1_Estimated = X1.Minus(tmp2);//估值
IMatrix tmp3 = tmp1.Multiply(A).Multiply(this.Adjustment.CovaOfEstimatedParam);
IMatrix new_CovaOfEstimatedParam = this.Adjustment.CovaOfEstimatedParam.Minus(tmp3);//协方差矩阵
IMatrix P2 = new ArrayMatrix(fixedIonoAmb.Count, fixedIonoAmb.Count, 0);
for (int kk = 0; kk < fixedIonoAmb.Count; kk++)
{
P2[kk, kk] = 5 * Math.Pow(10, 6); //给FCB的一个权值，暂定5*10^6
}
IMatrix m1 = P2.GetInverse();
IMatrix m2 = m1.Plus(AQAT);
IMatrix m3 = m2.GetInverse();
IMatrix J = this.Adjustment.CovaOfEstimatedParam.Multiply(AT).Multiply(m3);
IMatrix M4 = J.Multiply(W);
IMatrix new2_Estimated = this.Adjustment.Estimated;
IMatrix new2_CovaOfEstimatedParam = this.Adjustment.CovaOfEstimatedParam;
if (Math.Sqrt(M4[0, 0] * M4[0, 0] + M4[1, 0] * M4[1, 0] + M4[2, 0] * M4[2, 0]) < 0.001)
{
new2_Estimated = this.Adjustment.Estimated;
new2_CovaOfEstimatedParam = this.Adjustment.CovaOfEstimatedParam;
}
else
{
new2_Estimated = X1.Minus(M4);
IMatrix m5 = J.Multiply(A).Multiply(this.Adjustment.CovaOfEstimatedParam);
new2_CovaOfEstimatedParam = this.Adjustment.CovaOfEstimatedParam.Minus(m5);
}
this.Adjustment.Estimated_PPPAR1 = new WeightedVector(new1_Estimated, new_CovaOfEstimatedParam);
this.Adjustment.Estimated_PPPAR2 = new WeightedVector(new2_Estimated, new2_CovaOfEstimatedParam);
#endregion
}
#endregion
}
#endregion
}
#endregion
return(result);
}
/// <summary>
/// 迭代求解
/// </summary>
/// <param name="epochInfo"></param>
/// <param name="lastResult"></param>
/// <returns></returns>
public override SingleSiteGnssResult CaculateKalmanFilter(
EpochInformation epochInfo,
SingleSiteGnssResult lastResult = null)
{
return(Caculate(epochInfo));
}
public override SingleSiteGnssResult CaculateKalmanFilter(EpochInformation epochInfo, SingleSiteGnssResult lastResult)
{
// return Caculate(epochInfo);
return(base.CaculateKalmanFilter(epochInfo, lastResult));
}
/// <summary>
/// 滤波计算
/// </summary>
/// <param name="epochInfo"></param>
/// <param name="lastResult"></param>
/// <returns></returns>
public override SingleSiteGnssResult CaculateKalmanFilter(EpochInformation epochInfo, SingleSiteGnssResult lastResult)
{
var result = base.CaculateKalmanFilter(epochInfo, lastResult) as RecursiveIonoFreePppResult;
return(result);
}
/// <summary>
/// 滤波计算
/// </summary>
/// <param name="epochInfo"></param>
/// <param name="lastResult"></param>
/// <returns></returns>
public override SingleSiteGnssResult CaculateKalmanFilter(EpochInformation epochInfo, SingleSiteGnssResult lastResult)
{
//极速模式。
if (Option.TopSpeedModel)
{
return(base.CaculateKalmanFilter(epochInfo, lastResult));
}
epochInfo.RemoveIonoFreeUnavailable();
if (epochInfo.Count < 2)
{
log.Error("卫星可用数量不足：" + epochInfo.Count);
return(null);
}
var result = base.CaculateKalmanFilter(epochInfo, lastResult) as PppResult;
//外部模糊度文件直接固定
if (Option.IsFixingAmbiguity && Option.IsUseFixedParamDirectly && File.Exists(Option.AmbiguityFilePath) && Option.IsUsingAmbiguityFile)
{
return(result);
}
//var testBasePrn = new SatelliteNumber(12, SatelliteType.G);
//平滑MW
if (Option.IsFixingAmbiguity)
{
SmoothMwProvider.Add(epochInfo);
}
if (this.IsFixingAmbiguity
//&& epochInfo.Contains(testBasePrn)
&& epochInfo.ReceiverTime.DateTime.TimeOfDay > TimeSpan.FromHours(2)
)
{
//this.CurrentBasePrn = testBasePrn;
var time = epochInfo.ReceiverTime;
var SD_MW = SmoothMwProvider.GetDifferMwValue(CurrentBasePrn); //周为单位
if (SD_MW.Count < 2) //MW精度不足
{
return(result);
}
var wmFcb = WideLaneBiasService.Get(time).GetMwDiffer(CurrentBasePrn); //用于模糊度固定，精度要求不高，一次任务获取一次即可
SatWideNarrowValueManager wideNarrowValues = new SatWideNarrowValueManager(); //存储用于模糊度固定的宽窄项及对应的浮点产品
//基本参数定义
double maxMwRms = 0.3; //MW 至少的平滑精度
double maxDevOfInt = 0.2; //MW 取整时允许最大的偏差
var f1 = Frequence.GetFrequenceA(CurrentBasePrn, time).Value; // 1575.42;
var f2 = Frequence.GetFrequenceB(CurrentBasePrn, time).Value; // 1227.60;
var wideWaveLen = Frequence.GetMwFrequence(CurrentBasePrn, time).WaveLength;
var narrowWaveLen = Frequence.GetNarrowLaneFrequence(CurrentBasePrn, time).WaveLength;
#region 对MW值（宽巷模糊度）进行固定
foreach (var item in SD_MW)
{
//简单的质量控制
if (item.Value.Rms > maxMwRms && !Double.IsNaN(item.Value.Rms))
{
continue;
}
var f = wmFcb[item.Key]; //已经归算为和WM的定义相同，在[0-1]区间，满足 B=N+f, N=B-f.
if (f == double.NaN)
{
continue;
}
var B = item.Value.Value;
var nearN = B - f;
var N = (int)Math.Round(nearN); //直接进行取整数
if (Math.Abs(nearN - N) <= maxDevOfInt) //fixedSD_MW=0代表无效
{
wideNarrowValues.GetOrCreate(item.Key).WideLane = new IntFractionNumber(f, N); //改正后的 WLFCB
}
}
#endregion
//提取浮点解星间单差
SetPppFloatAmbiguityLen(result, wideNarrowValues);
#region 固定窄巷模糊度 //星间单差，窄巷模糊度
var tempCoeef = f2 / (f1 + f2); //算法1
var tempCoeef2 = (f1 + f2) * 1e6 / GnssConst.LIGHT_SPEED; //算法2：单位转换,窄巷波长的倒数
var tempCoeef3 = f2 / (f1 - f2); //算法2
foreach (var kv in wideNarrowValues.Data)
{
// var nFcb = SDNL_FCB[kv];//获取窄巷的产品
var prn = kv.Key;
var satData = kv.Value;
var f = NarrawLaneFcbService.GetBsdOfNarrowLane(prn, CurrentBasePrn, time); //获取窄巷的产品f, 符合B=N+f
if (!RmsedNumeral.IsValid(f))
{
continue;
}
var wideInt = satData.WideLane.Int;//只需要整数部分，小数部分化为窄巷小数部分，不影响整体
var floatPppAmbiLen = satData.FloatAmbiguityLength;
var B = (floatPppAmbiLen - tempCoeef * wideWaveLen * wideInt) / narrowWaveLen;//算法1
#region 算法验证
var B2 = tempCoeef2 * floatPppAmbiLen - tempCoeef3 * wideInt; //算法2
var differ = B - B2;
if (differ > 0.0001)
{
int ii = 0;
throw new Exception("Error!");
}
#endregion
var nearN = B - f;
var N = (int)Math.Round(nearN.Value); //直接进行取整数
if (Math.Abs(nearN.Value - N) <= maxDevOfInt) //fixedSD_MW=0代表无效
{
satData.NarrowLane = new IntFractionNumber(f.Value, N); //改正后的 WLFCB
}
}
#endregion
//提取协方差，采用Lambda去相关后固定模糊度
#region LAMBDA固定窄巷模糊度
//提取已选择的模糊度协方差阵
//#region 星间单差窄巷模糊度协方差矩阵
//IMatrix B = new ArrayMatrix(this.Adjustment.ParamNames.Count - 6, this.Adjustment.ParamNames.Count - 5, 0.0); //将非差模糊度转换为星间单差模糊度的旋转矩阵
//int index = 0;//参考星的索引位置???,参考星不同，index不同
//for (int i = 5; i < this.Adjustment.ParamNames.Count; i++)
//{
// SatelliteNumber sat = SatelliteNumber.Parse(this.Adjustment.ParamNames[i].Substring(0, 3));
// if (sat == MaxPrn)
// {
// break;
// }
// else
// {
// index++;
// }
//}
//for (int i = 0; i < this.Adjustment.ParamNames.Count - 6; i++)
//{
// if (i < index)
// {
// B[i, index] = -1; B[i, i] = 1;
// }
// if (i >= index)
// {
// B[i, index] = -1; B[i, i + 1] = 1;
// }
//}
//IMatrix BT = B.Transposition;
//IMatrix covaAmb = new ArrayMatrix(this.Adjustment.ParamNames.Count - 5, this.Adjustment.ParamNames.Count - 5, 0); //非差模糊度的协方差矩阵
//for (int i = 0; i < this.Adjustment.ParamNames.Count - 5; i++)
//{
// for (int j = 0; j < this.Adjustment.ParamNames.Count - 5; j++)
// {
// covaAmb[i, j] = this.Adjustment.CovaOfEstimatedParam[i + 5, j + 5];
// }
//}
//IMatrix SD_covaAmb = B.Multiply(covaAmb).Multiply(BT); //星间单差模糊度的协方差矩阵
//IMatrix SDNL_covaAmb = new ArrayMatrix(this.Adjustment.ParamNames.Count - 6, this.Adjustment.ParamNames.Count - 6, 0.0); //星间单差窄巷模糊度的协方差矩阵
//for (int i = 0; i < this.Adjustment.ParamNames.Count - 6; i++)
//{
// for (int j = 0; j < this.Adjustment.ParamNames.Count - 6; j++)
// {
// SDNL_covaAmb[i, j] = SD_covaAmb[i, j] * (1575.42 + 1227.60) / (1575.42 * GnssConst.GPS_L1_WAVELENGTH);//系数
// }
//}
//#endregion
//#region 部分模糊度固定，先将可以固定的星间单差窄巷模糊度 及其 协方差矩阵挑出来
//List<string> old_ambpara = new List<string>();
//foreach (var item in SD_floatAmb.Keys)
//{
// old_ambpara.Add(item.ToString());//所有的模糊度（除了参考星）
//}
//List<string> new_ambpara = new List<string>();
//foreach (var item in SDNL_floatAmb.Keys)
//{
// new_ambpara.Add(item.ToString());//可以固定的模糊度（宽巷 < 0.25 && NLFCB != 0）
//}
//IMatrix newtrix = NamedMatrix.GetNewMatrix(new_ambpara, old_ambpara, SDNL_covaAmb.Array); //将可以固定的模糊度的协方差矩阵挑出来
//#endregion
#endregion
StringBuilder sb = new StringBuilder();
sb.Append("PPP模糊度固定，" + time + ", BasePrn : " + CurrentBasePrn + ", 其它：" + wideNarrowValues.Count + ", " + Geo.Utils.StringUtil.ToString(wideNarrowValues.Keys));
log.Info(sb.ToString());
//计算固定后的模糊度距离
//已经固定的模糊度
var fixedPppAmbi = new Dictionary <SatelliteNumber, double>();
foreach (var kv in wideNarrowValues.Data)
{
if (!kv.Value.IsValid)
{
continue;
}
var prn = kv.Key;
var satData = kv.Value;
var wideInt = satData.WideLane.Int; //宽巷模糊度仍然采用整数
var floatNarrowAmbi = satData.NarrowLane.Value; //浮点部分包括在窄巷硬件延迟中
var fixedAmbiLen = tempCoeef * wideWaveLen * wideInt + narrowWaveLen * floatNarrowAmbi;
satData.FixedAmbiguityLength = fixedAmbiLen;
//简单质量控制
if (satData.DifferOfAmbiguityLength < 0.5)
{
fixedPppAmbi[prn] = fixedAmbiLen;
}
}
#region 条件平差 模糊度固定
return(FixPppResult(result, fixedPppAmbi));
#endregion
}
return(result);
}