public override void Correct(EpochSatellite epochSatellite)
{
if (IonoParamService == null)
{
return;
}
IIonoParam ionParam = IonoParamService.Get(epochSatellite.ReceiverTime);
if (ionParam == null)
{
return;
}
double correction = GetCorrectorInDistance(epochSatellite, ionParam);
this.Correction = correction;
if (IsCorrectionOnPhase)
{
this.Correction = -this.Correction;
}
// epochSatellite.SetCorrection(ErrorType, correction);
}
/// <summary>
/// 改正
/// </summary>
/// <param name="TInput"></param>
public override void Correct(EpochSatellite TInput)
{
this.Corrections = new Dictionary <string, Dictionary <RinexSatFrequency, double> >();
//执行改正
foreach (var item in this.Correctors)
{
item.Correct(TInput);
this.Corrections.Add(item.Name, item.Correction);
}
var vals = new Dictionary <RinexSatFrequency, double>();
foreach (var item in Correctors)
{
if (item == null || item.Correction == null)
{
continue;
}
foreach (var subitem in item.Correction)
{
if (!vals.ContainsKey(subitem.Key))
{
vals[subitem.Key] = 0.0;
}
vals[subitem.Key] += subitem.Value;
}
}
//可在此设断点查看各个改正情况。
this.Correction = (vals);
}
public override void Correct(EpochSatellite sat)
{
if (IonoService == null)
{
return;
}
foreach (var kv in sat.Data)
{
double slopeDelayFreq = GetGridModelCorrection(sat, kv.Key, IonoService);
kv.Value.AddRangeCorrection(ParamNames.Iono, slopeDelayFreq);
kv.Value.AddPhaseCorrection(ParamNames.Iono, -1.0 * slopeDelayFreq);//码延迟，波超前
}
//double slopeDelayFreqA = GetGridModelCorrection(sat, FrequenceType.A, IonoService);
//sat.FrequenceA.AddRangeCorrection( ParamNames.Iono, slopeDelayFreqA);
//sat.FrequenceA.AddPhaseCorrection(ParamNames.Iono, -1.0 * slopeDelayFreqA);//码延迟，波超前
//if (sat.Contains(FrequenceType.B))
//{
// double slopeDelayFreqB = GetGridModelCorrection(sat, FrequenceType.B, IonoService);
// //1.0 * GetIonoDelayRange(tec.Value, sat.FrequenceB.Frequence.Value);//斜方向延迟
// sat.FrequenceB.AddRangeCorrection(ParamNames.Iono, slopeDelayFreqB);
// sat.FrequenceB.AddPhaseCorrection(ParamNames.Iono, -1.0 * slopeDelayFreqB);//码延迟，波超前
//}
this.Correction = 0;
}
/// <summary>
/// 探测
/// </summary>
/// <param name="epochSat"></param>
/// <returns></returns>
public bool Detect(EpochSatellite epochSat)
{
bool isRecordedCycleSlipe = epochSat.EpochInfo.EpochState == EpochState.CycleSlip || epochSat.FrequenceA.IsPhaseLossedLock || epochSat.FrequenceB.IsPhaseLossedLock;
var alreadyHasSlip = IsUsingRecordedCsInfo && isRecordedCycleSlipe;
//double f1 = epochSat.FrequenceA.Frequence.Value * 1E6;
//double f2 = epochSat.FrequenceB.Frequence.Value * 1E6;
//double lam2 = GnssConst.LIGHT_SPEED / (f1 - f2);
double lam = epochSat.Combinations.MwRangeCombination.Frequence.WaveLength;
MaxNumLambdas = 1; //去掉之，会造成误探
lambdaLimit = MaxNumLambdas * lam;
bool isCS = GetDetection(epochSat.RecevingTime, epochSat.Prn,
epochSat.Combinations.MwRangeCombination.Value, alreadyHasSlip);
if (isCS)
{
CycleSlipStorage.Regist(epochSat.Prn.ToString(), epochSat.Time.Value);
}
if (epochSat.Time.Value.Hour == 2 && epochSat.Time.Value.Minute == 27)
{
int a = 0;
}
return(isCS);
}
/// <summary>
/// 观测量的权逆阵，一个对角阵。按照先伪距，后载波的顺序排列。
/// 标准差由常数确定，载波比伪距标准差通常是 1:100，其方差比是 1:10000.
/// 1.0/140*140=5.10e-5
/// </summary>
/// <param name="factor">载波和伪距权逆阵因子（模糊度固定后，才采用默认值！）</param>
/// <returns></returns>
protected IMatrix BulidInverseWeightOfObs()
{
int satCount = EnabledSatCount;// EpochInfo.EnabledSatCount;
int row = satCount * 2;
// double[][] inverseWeight = Geo.Utils.MatrixUtil.Create(row);
DiagonalMatrix inverseWeight = new DiagonalMatrix(row, 1);
double [] inverseWeightVector = inverseWeight.Vector;
double invFactorOfRange = 1;
double invFactorOfPhase = Option.PhaseCovaProportionToRange;
// double invFactorOfPhase = 0.003 * 0.003;
int i = 0;
foreach (var prn in CurrentMaterial.EnabledPrns)// 一颗卫星1行
{
EpochSatellite e = this.CurrentMaterial[prn];
double inverseWeightOfSat = SatWeightProvider.GetInverseWeightOfRange(e);
//如果具有周跳，则权值同伪距 ？？？
//if (e.HasCycleSlip)
// invFactorOfPhase = invFactorOfRange;
//inverseWeight[time][time] =8.87 * inverseWeightOfSat * invFactorOfRange; //8.87是按误差传播定律算的组合值的方差
//inverseWeight[time + satCount][time + satCount] = 8.87 *inverseWeightOfSat * invFactorOfPhase;
inverseWeightVector[i] = inverseWeightOfSat * invFactorOfRange;
inverseWeightVector[i + satCount] = inverseWeightOfSat * invFactorOfPhase;
i++;
}
return(inverseWeight);
}
/// <summary>
/// 直接返回，不做检查。
/// </summary>
/// <param name="satelliteType"></param>
/// <returns></returns>
public long GetCycle(EpochSatellite sat)
{
SatelliteNumber prn = sat.Prn;
var key = GetKey(sat.EpochInfo.SiteName, sat.Prn);
return(this[key]);
}
/// <summary>
/// 权逆，方差。
/// </summary>
/// <param name="satellite">测站卫星向量</param>
/// <returns></returns>
public double GetInverseWeightOfRange(EpochSatellite satellite)
{
double standardCova = 1;//标准差//伪距以10米为基准
double invWeight = standardCova * 1.0 / GetWeight(satellite);
return(invWeight);// *100000;
}
public override void Correct(EpochSatellite epochSatellite)
{
IEphemeris sat = epochSatellite.Ephemeris;
double correction = GetRelativeCorrection(sat) * GnssConst.LIGHT_SPEED;
this.Correction = (correction);
}
public override void Correct(EpochSatellite epochSatellite)
{
Time gpsTime = epochSatellite.RecevingTime;
IEphemeris sat = epochSatellite.Ephemeris;
SatelliteNumber prn = epochSatellite.Prn;
//计算太阳位置方法
//XYZ sunPosition = epochSatellite.EpochInfo.DataSouceProvider.UniverseObjectProvider.GetSunPosition(gpsTime);
//新的计算太阳位置方法
Time tutc = gpsTime.GpstToUtc();
//查找地球自转信息
Gnsser.Data.ErpItem erpv = null;
if (DataSouceProvider.ErpDataService != null)
{
erpv = DataSouceProvider.ErpDataService.Get(tutc);
}
if (erpv == null)
{
erpv = ErpItem.Zero;
}
XYZ sunPosition = new XYZ();
DataSouceProvider.UniverseObjectProvider.GetSunPosition(gpsTime, erpv, ref sunPosition);
//use L1 value
IAntenna antenna = DataSouceProvider.AntennaDataSource.Get(prn.ToString(), gpsTime);
if (antenna == null)
{
return;
}
string AntennaType = antenna.AntennaType;
XYZ svPos = sat.XYZ;
XYZ receiverPosition = epochSatellite.SiteInfo.EstimatedXyz;
if (receiverPosition.Equals(XYZ.Zero))
{
return;
}
bool cycleSlip = epochSatellite.IsUnstable;
if (cycleSlip || !PhaseManager.Contains(prn)) //a cycle slip happend
{
PhaseManager[prn] = new SatVectorPhase();
}
double windUpCorrection = GetSatPhaseWindUpCorectValue(prn, gpsTime, svPos, receiverPosition, sunPosition, AntennaType);
//double windUpCorrection2 = GetSatPhaseWindUpCorectValue(satelliteType, gpsTime, svPos, receiverPosition, epochSatellite, sunPosition);
this.Correction = (windUpCorrection);
}
/// <summary>
/// 获取权值
/// </summary>
/// <param name="satellite">测站卫星向量对象</param>
/// <returns></returns>
public double GetWeight(EpochSatellite satellite)
{
//避免低高度角卫星的权太小，czs,2018.09.27, hmx
double sinE = 0.5 + 0.5 * Math.Sin(satellite.Polar.Elevation * AngularConvert.DegToRadMultiplier);
//距离始终为1，错误：2015.05.07，cy，发现。待修改,先替换为 satellite.ApproxXyzVector.Length
//2015.05.10,czs, 已经找到原因，是因为后续验证错误，而源代码无误，已修改
double range = satellite.EstmatedVector.Length;// satellite.Polar.Range;
if (range == 0)
{
range = 1;
}
GnssSystem sys = GnssSystem.GetGnssSystem(satellite.Prn.SatelliteType);
double factor = range / sys.ApproxRadius;
double Weight = Math.Pow(sinE / factor, 2.0);
//防止测站坐标为 0 的情况
if (Geo.Utils.DoubleUtil.IsValid(Weight))
{
return(Weight);
}
return(1);
}
/// <summary>
/// 构造函数
/// </summary>
/// <param name="sat"></param>
public EpochSatAnlysisInfo(EpochSatellite sat)
{
this.Prn = sat.Prn;
//计算多路径因子
var freA = sat.FrequenceA;
var freB = sat.FrequenceB;
var alfaTemp = (freA.Frequence.Value / freB.Frequence.Value);
var alfa = alfaTemp * alfaTemp;
var factorL1 = GetMultpathFactorL1(freA.PseudoRange.Value, freA.PhaseRange.Value, freB.PhaseRange.Value, alfa);
var factorL2 = GetMultpathFactorL2(freA.PseudoRange.Value, freA.PhaseRange.Value, freB.PhaseRange.Value, alfa);
this.MultipathFactors = new Dictionary <FrequenceType, double>();
MultipathFactors.Add(freA.FrequenceType, factorL1);
MultipathFactors.Add(freB.FrequenceType, factorL2);
//判断是否有效
InvalidCodes = new Dictionary <FrequenceType, List <GnssCodeType> >();
foreach (var freq in sat)
{
var list = new List <GnssCodeType>();
InvalidCodes[freq.FrequenceType] = list;
foreach (var item in freq)
{
foreach (var item2 in item)
{
if (!IsValid(item2.Value))
{
list.Add(item2.GnssCodeType);
}
}
}
}
}
/// <summary>
/// 获取指定的卫星
/// </summary>
/// <param name="infos"></param>
/// <param name="firstSat"></param>
/// <param name="number"></param>
/// <returns></returns>
public List <EpochSatellite> GetSats(IBuffer <EpochInformation> infos, EpochSatellite firstSat, int number)
{
List <EpochSatellite> sats = new List <EpochSatellite>();
sats.Add(firstSat);
int i = 0;
foreach (var item in infos)
{
if (i >= number)
{
break;
}
if (item == null)
{
continue;
}
var sat = item.Get(firstSat.Prn);
if (sat != null)
{
sats.Add(sat);
}
i++;
}
return(sats);
}
/// <summary>
/// 探测
/// </summary>
/// <param name="sats"></param>
public void DetectOrMarkOld(List <EpochSatellite> sats)
{
//顺序反转
sats.Reverse();
StringBuilder sb = new StringBuilder();
int count = sats.Count;
int i = 0;
EpochSatellite prevSat = null;
int prevSatIndex = 0;
foreach (var sat in sats)
{
prevSatIndex = i - 1;
//逆序时，实质是对其前一个进行探测，注意与顺序（探测下一个）的区别。
if (!Detector.Detect(sat) && prevSatIndex >= 0)
{
prevSat = sats[prevSatIndex];
if (prevSat.IsUnstable)//没有周跳
{
sb.Append("," + prevSat.Time.Value.ToShortTimeString());
prevSat.IsUnstable = false;
}
}
i++;
}
if (sb.Length > 0)
{
sb.Insert(0, "移除周跳标记：" + sats[0].Prn);
log.Debug(sb.ToString());
}
}
/// <summary>
/// 观测量的权逆阵，一个对角阵。按照先伪距，后载波的顺序排列。
/// 标准差由常数确定，载波比伪距标准差通常是 1:100，其方差比是 1:10000.
/// 1.0/140*140=5.10e-5
/// </summary>
/// <returns></returns>
protected IMatrix BulidInverseWeightOfObs()
{
int row = ObsCount;
double[][] inverseWeight = Geo.Utils.MatrixUtil.Create(ObsCount);
double invFactorOfRange = 1;
double invFactorOfPhase = Option.PhaseCovaProportionToRange;
int epochIndex = 0;
foreach (var epoch in this.CurrentMaterial)
{
int satIndex = 0;
int epochStartIndex = this.EnabledSatCount * 2 * epochIndex;//当前历元参数起始编号
foreach (var prn in EnabledPrns)
{
int rangIndex = GetRangIndex(satIndex, epochStartIndex);
int phaseIndex = GetPhaseIndex(satIndex, epochStartIndex);
EpochSatellite e = epoch[prn];
double inverseWeightOfSat = SatWeightProvider.GetInverseWeightOfRange(e);
inverseWeight[rangIndex][rangIndex] = inverseWeightOfSat * invFactorOfRange;
inverseWeight[phaseIndex][phaseIndex] = inverseWeightOfSat * invFactorOfPhase;
satIndex++;
}
epochIndex++;
}
return(new Matrix(inverseWeight));
}
public override void Correct(EpochSatellite epochSatellite)
{
IEphemeris sat = epochSatellite.Ephemeris;
XYZ reXyz = epochSatellite.SiteInfo.EstimatedXyz;
double correction = GetRelativeCorrection(sat, reXyz);
this.Correction = (correction);
}
public override void Correct(EpochSatellite epochSatellite)
{
double correction = -GetTropoCorrectValue(
epochSatellite.RecevingTime,
epochSatellite.Ephemeris.XYZ,
epochSatellite.SiteInfo.EstimatedXyz);
this.Correction = -1.0 * (correction);
}
/// <summary>
/// 计算窄巷模糊度浮点解
/// </summary>
/// <param name="Sat"></param>
/// <param name="FloatAmbiguityDiffer"></param>
/// <param name="wideLaneInteger"></param>
/// <returns></returns>
public double GetNarrowLaneValue(EpochSatellite Sat, double FloatAmbiguityDiffer, int wideLaneInteger)
{
this.Sat = Sat;
//求乘数因子
var narrowLane =
(FloatAmbiguityMultiFactor * FloatAmbiguityDiffer
- WideLaneMultiFactor * wideLaneInteger);
return(narrowLane);
}
public override bool Revise(ref EpochSatellite obj)
{
var rangeA = obj.FrequenceA.PseudoRange.Value;
if (!WindowA.DifferCheckAddOrClear(obj.ReceiverTime, rangeA, MaxRmsTimes, 1))
{
//改正
return(false);
}
Previous = obj;
return(true);
}
public override void Correct(EpochSatellite epochSatellite)
{
XYZ receiverXyz = epochSatellite.SiteInfo.EstimatedXyz;
XYZ satXyz = epochSatellite.Ephemeris.XYZ;
XYZ ray = satXyz - receiverXyz;
double correction = ray.Radius();
this.Correction = (correction);
}
public override void Correct(EpochSatellite epochSatellite)
{
if (RefEpochInfo.Contains(epochSatellite.Prn))
{
//计算公共模型改正数
var sat = RefEpochInfo[epochSatellite.Prn];
var obs = sat[EpochSatDataType];
//这个改正数，直接添加到流动站的站星距离上，就不用再添加其它任何改正了。
this.Correction = obs.CorrectedValue //参考站伪距观测值
- sat.ApproxVector.Norm //参考站站星距离真值
+ RefClkError * GnssConst.LIGHT_SPEED; //参考站接收机距离误差
}
}
/// <summary>
/// 观测量的权逆阵，一个对角阵。
/// </summary>
/// <returns></returns>
protected IMatrix BulidInverseWeightOfObs( )
{
int satCount = CurrentMaterial.EnabledSatCount;
double[][] inverseWeight = Geo.Utils.MatrixUtil.Create(satCount);
for (int i = 0; i < satCount; i++)
{
EpochSatellite e = this.CurrentMaterial[i];
inverseWeight[i][i] = SatWeightProvider.GetInverseWeightOfRange(e);
}
return(new ArrayMatrix(inverseWeight));
}
/// <summary>
/// 数据是否有效
/// </summary>
/// <param name="sat"></param>
/// <returns></returns>
public bool IsValid(EpochSatellite sat)
{
if (!sat.Contains(FrequenceType.B))
{
return(false);
}
var isOk = sat.FrequenceA.PhaseRange.RawPhaseValue != 0 &&
sat.FrequenceA.PseudoRange.Value != 0 &&
sat.FrequenceB.PhaseRange.RawPhaseValue != 0 &&
sat.FrequenceB.PseudoRange.Value != 0;
return(isOk);
}
/// <summary>
/// 电离层模型改正。返回改正距离。 单位：米
/// </summary>
/// <param name="epochSatellite">卫星观测信息</param>
/// <param name="ionParam">电离层参数</param>
/// <returns></returns>
public static double GetCorrectorInDistance(EpochSatellite epochSatellite, IIonoParam ionParam)
{
if (ionParam == null)
{
return(0);
}
double correction = GetCorrectorInDistance(epochSatellite.RecevingTime.SecondsOfWeek,
epochSatellite.Ephemeris.XYZ,
epochSatellite.SiteInfo.EstimatedXyz,
ionParam);
return(correction);
}
public override void Correct(EpochSatellite epochSatellite)
{
IEphemeris sat = epochSatellite.Ephemeris;
if (sat == null)
{
return;
}
XYZ svPos = sat.XYZ;
XYZ nominalPos = epochSatellite.SiteInfo.EstimatedXyz;
double correction = GetSatGravDelayCorrection(svPos, nominalPos);
this.Correction = (correction);
}
/// <summary>
/// 设置改正的误差
/// </summary>
/// <param name="sat"></param>
/// <param name="frequenceType"></param>
private void SetPolyFitCorrection(EpochSatellite sat, FrequenceType frequenceType)
{
var prn = sat.Prn;
var sp = GetBufferValues(prn, frequenceType).GetPolyFitValue(sat.ReceiverTime, 2);
if (sp.Rms > 10)
{
log.Warn("多项式平滑伪距效果差！ RMS: " + sp.Rms);
return;
}
var correctionOfP = sp.Value - sat[frequenceType].PseudoRange.Value;
sat[frequenceType].PseudoRange.Corrections.Add("PolyFit", correctionOfP);
}
public override void Correct(EpochSatellite epochSatellite)
{
if (epochSatellite.Ephemeris == null)
{
return;
}
//卫星钟差δts所引起距离误差cδts，在这里变成了观测近似值的钟差距离，改正到观测值的近似值上，其符号为负，见公式推导。
double correction = -1.0 * GetClockBiasCorectValue(epochSatellite.Ephemeris.ClockBias);
epochSatellite.SetCommonCorrection(CorrectionNames.SatClockBiasDistance.ToString(), correction);
this.Correction = 0;// (correction);
}
private void InverseWeightOfEpoch(double[] inverseWeightVector, double invFactorOfRange, double invFactorOfPhase, EpochInformation epoch)
{
foreach (var prn in epoch.EnabledPrns)
{
EpochSatellite e = epoch[prn];
var SiteSatAmbiguityName = this.GnssParamNameBuilder.GetSiteSatAmbiguityParamName(e);
int IndexOfSiteSatAmbiguityName = ParamNames.IndexOf(SiteSatAmbiguityName);
int row = IndexOfSiteSatAmbiguityName - BaseCount;//行的索引号
int next = row + ObsCount;
double inverseWeightOfSat = SatWeightProvider.GetInverseWeightOfRange(e);
inverseWeightVector[row] = inverseWeightOfSat * invFactorOfRange;
inverseWeightVector[next] = inverseWeightOfSat * invFactorOfPhase;
}
}
/// <summary>
/// 观测量的权逆阵，一个对角阵。按照先伪距，后载波的顺序排列。
/// 标准差由常数确定，载波比伪距标准差通常是 1:100，其方差比是 1:10000.
/// 1.0/140*140=5.10e-5
/// </summary>
/// <returns></returns>
protected IMatrix BulidInverseWeightOfObs()
{
int row = CurrentMaterial.EnabledSatCount;
double[][] inverseWeight = Geo.Utils.MatrixUtil.Create(row);
double invFactorOfRange = 1;
for (int i = 0; i < row; i++)
{
EpochSatellite e = this.CurrentMaterial[i];
double inverseWeightOfSat = SatWeightProvider.GetInverseWeightOfRange(e);
inverseWeight[i][i] = inverseWeightOfSat * invFactorOfRange;
}
return(new Matrix(inverseWeight));
}
/// <summary>
/// 获取电离层格网模型延迟
/// </summary>
/// <param name="sat"></param>
/// <param name="frequenceType"></param>
/// <param name="IonoService"></param>
/// <returns></returns>
public static double GetGridModelCorrection(EpochSatellite sat, FrequenceType frequenceType, IIonoService IonoService)
{
//计算穿刺点获取电离层浓度
var punctPoint = sat.GetIntersectionXyz(IonoService.HeightOfModel);
var geocentricLonLat = Geo.Coordinates.CoordTransformer.XyzToSphere(punctPoint);
var tec = IonoService.GetSlope(sat.ReceiverTime, geocentricLonLat, sat.SphereElevation);
if (tec == null || tec.Value == 0)
{
return(0);
}
Frequence freq = sat[frequenceType].Frequence;
double slopeDelayFreqA = 1.0 * GetIonoDelayRange(tec.Value, freq.Value);//斜方向延迟
return(slopeDelayFreqA);
}
/// <summary>
/// 观测量的权逆阵，一个对角阵。按照先伪距，后载波的顺序排列。
/// 标准差由常数确定，载波比伪距标准差通常是 1:100，其方差比是 1:10000.
/// 1.0/140*140=5.10e-5
/// </summary>
/// <param name="factor">载波和伪距权逆阵因子（模糊度固定后，才采用默认值！）</param>
/// <returns></returns>
public IMatrix BulidInverseWeightOfObs()
{
//double[][] inverseWeight = Geo.Utils.MatrixUtil.Create(ObsCount * 2);
DiagonalMatrix inverseWeight = new DiagonalMatrix(ObsCount * 2);
double[] inverseWeightVector = inverseWeight.Vector;
double invFactorOfRange = 1;
double invFactorOfPhase = Option.PhaseCovaProportionToRange;
int index1 = 0;
foreach (var epoch in this.CurrentMaterial)
{
foreach (var prn in epoch.EnabledPrns)
{
EpochSatellite e = epoch[prn];
double inverseWeightOfSat = SatWeightProvider.GetInverseWeightOfRange(e);
inverseWeight[index1] = inverseWeightOfSat * invFactorOfRange;
inverseWeight[index1 + ObsCount] = inverseWeightOfSat * invFactorOfPhase;
index1++;
}
}
//start = DateTime.Now;
//int index = 0;
//foreach (var epoch in this.CurrentMaterial)
//{
// foreach (var prn in epoch.EnabledPrns)
// {
// EpochSatellite e = epoch[prn];
// double inverseWeightOfSat = SatWeightProvider.GetInverseWeight(e);
// //inverseWeight[index][index] = inverseWeightOfSat * invFactorOfRange;
// //inverseWeight[index + ObsCount][index + ObsCount] = inverseWeightOfSat * invFactorOfPhase;
// inverseWeight[index, index] = inverseWeightOfSat * invFactorOfRange;
// inverseWeight[index + ObsCount, index + ObsCount] = inverseWeightOfSat * invFactorOfPhase;
// index++;
// }
//}
//span = DateTime.Now - start;
//Console.WriteLine(span.TotalMilliseconds + "ms计算Q-");
//var ss = inverseWeight - inverseWeight1;
return(inverseWeight);
}
