/// <summary>
/// 实时计算，数据计算
/// </summary>
/// <param name="input">观测矩阵</param>
/// <returns></returns>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
if (input == this.ObsMatrix)
{
return(this.LastResult);
}
this.ObsMatrix = input;
switch (StepOfRecursive)
{
case StepOfRecursive.SuperposOfConstNeq:
return(GetConstParamResult(input, NormalEquationSuperposer));
//case StepOfRecursive.ComputeMutableParam: break;
case StepOfRecursive.SequentialConst:
AdjustResultMatrix res = GetSequentialConst(input);
this.LastConstResult = res;
return(res);
case StepOfRecursive.RealTime: //实时计算，参数变化可能带来错误
AdjustResultMatrix result = GetRealTimeResult(input);
this.LastResult = result;
return(result);
case StepOfRecursive.ParamAdjust: //参数逐历元平差
return(GetSimpleParamAdjustResult(input)); //参数平差验证,2018.10.15, czs, hmx, 验证第一个结果是一样的。
default:
break;
}
return(null);
}
/// <summary>
/// 计算
/// </summary>
/// <param name="input"></param>
/// <returns></returns>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
//try
//{
this.ObsMatrix = input;
int paramCount = input.Coefficient.ColCount;
Predict(input.Transfer, InverseWeightOfTransfer);
var est1 = CorrectSimple(input.Observation - input.FreeVector, input.Coefficient, input.Observation.InverseWeight);
// var est2 = CorrectNormal(this.Observation, CoeffOfParam, Observation.InverseWeight);
// var est3 = NewCorrect(this.Observation, CoeffOfParam, Observation.InverseWeight);
//var differ12 = est1 - est2;
//var differ23 = est2 - est3;
//var differ13 = est1 - est3;
var Estimated = est1;
AdjustResultMatrix result = new AdjustResultMatrix()
.SetEstimated(Estimated)
.SetObsMatrix(input)
.SetFreedom(Freedom)
.SetVarianceFactor(VarianceOfUnitWeight)
.SetVtpv(vtpv)
;
return(result);
//}
//catch (Exception ex)
//{
// log.Error("SimpleKalmanFilter 滤波出错了", ex);
//}
}
/// <summary>
/// 构造函数
/// </summary>
/// <param name="obs"></param>
/// <param name="PrevAdjustment"></param>
public OneDimAdjustMatrixBuilder(RmsedNumeral obs, AdjustResultMatrix PrevAdjustment = null)
{
this.PrevAdjustment = PrevAdjustment;
this.ObsValue = obs;
this.ParamNames = new List <string>()
{
"Value"
};
}
private AdjustResultMatrix GetRealTimeResult(AdjustObsMatrix input)
{
//构建不变参数的法方程
var newConstParamNe = input.BuildConstParamNormalEquation();
NormalEquationSuperposer.Add(newConstParamNe); //添加到法方程迭加器中
WeightedVector estY = NormalEquationSuperposer.GetEstimated();
AdjustResultMatrix result = Step3GetMutableX(estY, ObsMatrix); //求异变参数
return(result);
}
/// <summary>
/// 计算
/// </summary>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
//命名规则：0表示上一个，1表示预测，无数字表示当次
//上次次观测设置
var X0 = new Matrix((IMatrix)input.Apriori);
var Qx0 = new Matrix(input.Apriori.InverseWeight);
var Px0 = new Matrix(Qx0.GetInverse());
//本次观测设置
var Qo = new Matrix(input.Observation.InverseWeight);
var Po = new Matrix(Qo.GetInverse());
var A = new Matrix(input.Coefficient);
var AT = A.Trans;
var L = new Matrix((IMatrix)input.Observation);
int paramCount = A.ColCount;
int obsCount = A.RowCount;
//1.预测残差
//计算预测残差
var V1 = L - A * X0;//观测值 - 估计近似值
var Qv1 = Qo + A * Qx0 * AT;
//2.计算增益矩阵
var J = Qx0 * AT * Qv1.Inversion;// 增益矩阵
//3.平差结果
var dX = J * V1;
var X = X0 + dX;
//4.精度评定
var Qx = Qx0 - J * A * Qx0;
var Freedom = input.Observation.Count - input.ParamCount + input.Apriori.Count;
var V = A * dX - V1; //估值-观测值 V = A * X - L = A * (X0 + deltaX) - (l + A * X0) = A * deltaX - l.
var vtpv = (V.Trans * Po * V)[0, 0];
var VarianceOfUnitWeight = vtpv / Freedom; //单位权方差
var Estimated = new WeightedVector(X, Qx)
{
ParamNames = input.ParamNames
};
AdjustResultMatrix result = new AdjustResultMatrix()
.SetEstimated(Estimated)
.SetFreedom(Freedom)
.SetObsMatrix(input)
.SetVarianceFactor(VarianceOfUnitWeight)
.SetVtpv(vtpv)
;
return(result);
}
/// <summary>
/// 运行
/// </summary>
/// <param name="input"></param>
/// <returns></returns>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
//原始输入
Matrix B = new Matrix(input.Coefficient);
Matrix L = new Matrix((IMatrix)input.Observation);
Matrix QL = new Matrix((IMatrix)input.Observation.InverseWeight);
Matrix B0 = input.HasFreeVector ? new Matrix(input.FreeVector, true) : null;//B0
Matrix PL = QL.Inversion;
int freedom = B.RowCount;
Matrix BT = B.Trans;
int obsCount = L.RowCount;
int paramCount = 0;
Matrix W = -(B * L - B0);
Matrix N = B * QL * BT;
Matrix inverN = N.Inversion;
Matrix K = inverN * W;
Matrix Vhat = (QL * BT * K);
Matrix Qvhat = QL * BT * inverN * B * QL;
WeightedVector estLW = new WeightedVector(Vhat, Qvhat)
{
ParamNames = input.Observation.ParamNames
};
Matrix Lhat = L + Vhat;
Matrix QhatL = QL - Qvhat;
WeightedVector correctedObs = new WeightedVector(Lhat, QhatL)
{
ParamNames = input.Observation.ParamNames
};
double vtpv = (Vhat.Trans * PL * Vhat).FirstValue;
double s0 = vtpv / freedom;//单位权中误差估值
if (!DoubleUtil.IsValid(s0))
{
log.Error("方差值无效！" + s0);
}
AdjustResultMatrix result = new AdjustResultMatrix()
.SetAdjustmentType(AdjustmentType.条件平差)
.SetEstimated(estLW)
.SetCorrectedObs(correctedObs)
.SetObsMatrix(input)
.SetFreedom(freedom)
.SetVarianceFactor(s0)
.SetVtpv(vtpv);
return(result);
}
/// <summary>
/// 添加一个。
/// </summary>
/// <param name="Adjustment"></param>
public void AddAdjustment(AdjustResultMatrix Adjustment)
{
string name = "";
if (Adjustment.ObsMatrix.Tag is Time)
{
var epoch = (Time)Adjustment.ObsMatrix.Tag;
name = epoch.ToString();
//var table = AdjustTables.GetOrCreate(AdjustName.Epoch);
//table.NewRow();
//table.AddItem(AdjustName.Epoch, epoch);
}
var obsMatrixEq = Adjustment.ObsMatrix.GetObsMatrixEquation(name);
AdjustTables.Add(obsMatrixEq.Name, obsMatrixEq);
}
/// <summary>
/// 批量总体技术。参数不要改变，否则达不到预期效果。
/// </summary>
/// <param name="inputs"></param>
/// <returns></returns>
public List <AdjustResultMatrix> Run(List <AdjustObsMatrix> inputs)
{
AdjustObsMatrix firstMatrix = inputs[0];
Matrix Y0all = firstMatrix.HasSecondApprox ? new Matrix(firstMatrix.SecondApproxVector, true) : null;
WeightedVector estY = GetConstY(inputs);
//Matrix Y = Y0all + constY;
//step 3：求易变参数
List <AdjustResultMatrix> results = new List <AdjustResultMatrix>();
foreach (var obsMatrix in inputs)
{
this.ObsMatrix = obsMatrix;
AdjustResultMatrix result = Step3GetMutableX(estY, obsMatrix);
results.Add(result);
}
return(results);
}
/// <summary>
/// 添加一个。
/// </summary>
/// <param name="Adjustment"></param>
public void AddAdjustment(AdjustResultMatrix Adjustment)
{
#region 必须的
if (Adjustment.ObsMatrix.Tag is Time)
{
var epoch = (Time)Adjustment.ObsMatrix.Tag;
var table = AdjustTables.GetOrCreate(AdjustName.Epoch);
table.NewRow();
table.AddItem(AdjustName.Epoch, epoch);
}
AdjustTables.GetOrCreate(AdjustName.ParamName).NewRow((List <string>)Adjustment.ParamNames);
AdjustTables.GetOrCreate(AdjustName.Obs).NewRow((IVector)Adjustment.ObsMatrix.Observation);
AdjustTables.GetOrCreate(AdjustName.RmsOfObs).NewRow(Adjustment.ObsMatrix.Observation.GetRmsVector());
AdjustTables.GetOrCreate(AdjustName.Design).NewRow(Adjustment.ObsMatrix.Coefficient);
#endregion
if (!Vector.IsEmpty(Adjustment.ObsMatrix.ApproxVector))
{
AdjustTables.GetOrCreate(AdjustName.Approx).NewRow(Adjustment.ObsMatrix.ApproxVector);
}
if (!Matrix.IsEmpty(Adjustment.ObsMatrix.Transfer))
{
AdjustTables.GetOrCreate(AdjustName.Trans).NewRow(Adjustment.ObsMatrix.Transfer);
AdjustTables.GetOrCreate(AdjustName.RmsOfTrans).NewRow(Adjustment.ObsMatrix.Transfer.InverseWeight.Pow(0.5));
}
if (!Vector.IsEmpty(Adjustment.ObsMatrix.Apriori))
{
AdjustTables.GetOrCreate(AdjustName.Apriori).NewRow((IVector)Adjustment.ObsMatrix.Apriori);
AdjustTables.GetOrCreate(AdjustName.RmsOfApriori).NewRow(Adjustment.ObsMatrix.Apriori.GetRmsVector());
}
if (!Vector.IsEmpty((IVector)Adjustment.Estimated))
{
AdjustTables.GetOrCreate(AdjustName.Estimated).NewRow((IVector)Adjustment.Estimated);
AdjustTables.GetOrCreate(AdjustName.RmsOfEstimated).NewRow((IVector)Adjustment.Estimated.GetRmsVector());
}
}
/// <summary>
/// 构建电离层输出表格
/// </summary>
/// <param name="epoch"></param>
/// <param name="Adjustment"></param>
/// <param name="ParamNames"></param>
private void BuildIonoResult(ISiteSatObsInfo epoch, Geo.Algorithm.Adjust.AdjustResultMatrix Adjustment, List <string> ParamNames)
{
if (epoch is EpochInformation) //这里只处理单站单历元情况
{
var epochInfo = epoch as EpochInformation;
//电离层汇总
var allInOneTable = TableTextManager.GetOrCreate(epoch.Name + "_All_" + Gnsser.ParamNames.Iono);
allInOneTable.NewRow();
allInOneTable.AddItem("Epoch", epoch.ReceiverTime);
ObjectTableStorage tableIonoParam = null;
ObjectTableStorage tableIf = null;
ObjectTableStorage tableGridFile = null;
ObjectTableStorage tableHarmoFile = null;
ObjectTableStorage tableOfIonoParamService = null;
//电离层参数
if (Geo.Utils.StringUtil.Contanis(ParamNames, Gnsser.ParamNames.Iono, true))//参数化电离层文件
{
tableIonoParam = TableTextManager.GetOrCreate(epoch.Name + "_Param_" + Gnsser.ParamNames.Iono);
tableIonoParam.NewRow();
tableIonoParam.AddItem("Epoch", epoch.ReceiverTime);
var ionoResult = Adjustment.Estimated.GetAll(Gnsser.ParamNames.Iono);
foreach (var item in ionoResult)
{
//斜距转换为垂距
//计算穿刺点
var prn = SatelliteNumber.Parse(item.Key);
var sat = epochInfo.Get(prn);
tableIonoParam.AddItem(prn.ToString(), item.Value.Value);
}
}
//双频电离层
if (epochInfo.First.Count > 1)
{
tableIf = TableTextManager.GetOrCreate(epoch.Name + "_IFofC_" + Gnsser.ParamNames.Iono);
tableIf.NewRow();
foreach (var sat in epochInfo.EnabledSats)
{
//斜距转换为垂距
//计算穿刺点
var prn = sat.Prn;
var ionXyz = sat.GetIntersectionXyz();
var geoCoordOfIono = CoordTransformer.XyzToGeoCoord(ionXyz);
var ionoFreeRange = sat.Combinations.IonoFreeRange.Value;
var rangeA = sat.FrequenceA.PseudoRange.Value;
var ionoError = rangeA - ionoFreeRange;
tableIf.AddItem("Epoch", epoch.ReceiverTime);
tableIf.AddItem(prn.ToString(), ionoError);
}
}
if (Context.IgsGridIonoFileService != null && Context.IgsGridIonoFileService.TimePeriod.Contains(epoch.ReceiverTime))
{
tableGridFile = TableTextManager.GetOrCreate(epoch.Name + "_Grid_" + Gnsser.ParamNames.Iono);
tableGridFile.NewRow();
foreach (var sat in epochInfo.EnabledSats)
{
//斜距转换为垂距
//计算穿刺点
var prn = sat.Prn;
tableGridFile.AddItem("Epoch", epoch.ReceiverTime);
double val = IonoGridModelCorrector.GetGridModelCorrection(sat, FrequenceType.A, Context.IgsGridIonoFileService);
tableGridFile.AddItem(prn.ToString(), val);
}
}
if (Context.IgsCodeHarmoIonoFileService != null && Context.IgsCodeHarmoIonoFileService.TimePeriod.Contains(epoch.ReceiverTime))
{
tableHarmoFile = TableTextManager.GetOrCreate(epoch.Name + "_Harmo_" + Gnsser.ParamNames.Iono);
tableHarmoFile.NewRow();
foreach (var sat in epochInfo.EnabledSats)
{
//斜距转换为垂距
//计算穿刺点
var prn = sat.Prn;
tableHarmoFile.AddItem("Epoch", epoch.ReceiverTime);
double val = IonoGridModelCorrector.GetGridModelCorrection(sat, FrequenceType.A, Context.IgsCodeHarmoIonoFileService);
tableHarmoFile.AddItem(prn.ToString(), val);
}
}
if (Context.IonoKlobucharParamService != null)
{
var ionoParam = Context.IonoKlobucharParamService.Get(epoch.ReceiverTime);//
if (ionoParam != null)
{
tableOfIonoParamService = TableTextManager.GetOrCreate(epoch.Name + "_ParamModel_" + Gnsser.ParamNames.Iono);
tableOfIonoParamService.NewRow();
foreach (var sat in epochInfo.EnabledSats)
{
tableOfIonoParamService.AddItem("Epoch", epoch.ReceiverTime);
var val = IonoParamModelCorrector.GetCorrectorInDistance(sat, ionoParam);
tableOfIonoParamService.AddItem(sat.Prn.ToString(), val);
}
}
}
//保存到总表中
foreach (var sat in epochInfo.EnabledSats)
{
var prn = sat.Prn;
CheckAndAddIonoValueToMainTable(allInOneTable, tableIonoParam, prn, "Param");
CheckAndAddIonoValueToMainTable(allInOneTable, tableIf, prn, "IfofC");
CheckAndAddIonoValueToMainTable(allInOneTable, tableGridFile, prn, "Grid");
CheckAndAddIonoValueToMainTable(allInOneTable, tableHarmoFile, prn, "Harmo");
CheckAndAddIonoValueToMainTable(allInOneTable, tableOfIonoParamService, prn, "ParamModel");
}
}
}
/// <summary>
/// 数据计算
/// </summary>
/// <param name="input">观测矩阵</param>
/// <returns></returns>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
this.ObsMatrix = input;
//观测值权阵设置,对已知量赋值
Matrix L = new Matrix((IMatrix)input.Observation);
Matrix QL = new Matrix(input.Observation.InverseWeight);
Matrix PL = new Matrix(QL.GetInverse());
Matrix A = new Matrix(input.Coefficient);
Matrix AT = A.Trans;
Matrix X0 = input.HasApprox ? new Matrix(input.ApproxVector, true) : null;
Matrix D = input.HasFreeVector ? new Matrix(input.FreeVector, true) : null;
Matrix dN = input.CoeffIncrementOfNormalEquation;
int obsCount = A.RowCount;
int paramCount = A.ColCount;
int freedom = obsCount - paramCount;
//观测值更新
Matrix l = L - (A * X0 + D); //如果null，则是本身
//法方程
Matrix N = new Matrix(SymmetricMatrix.Parse(AT * PL * A + dN));
Matrix U = AT * PL * l;
Matrix InverN = N.Inversion;
//平差结果
Matrix x = InverN * U;
Matrix Qx = InverN - dN;
//精度评定
Matrix V = A * x - l;
Matrix Qv = QL - A * Qx * AT;
Matrix X = X0 + x;
double vtpv = (V.Trans * PL * V).FirstValue;
double s0 = vtpv / (freedom == 0 ? 0.1 : freedom);//单位权方差
WeightedVector estX = new WeightedVector(x, Qx)
{
ParamNames = input.ParamNames
};
WeightedVector CorrectedEstimate = new WeightedVector(X, Qx)
{
ParamNames = input.ParamNames
};
WeightedVector estV = new WeightedVector(V, Qv)
{
ParamNames = this.ObsMatrix.Observation.ParamNames
};
Matrix Lhat = L + V;
Matrix QLhat = A * Qx * AT;
var correctedObs = new WeightedVector(Lhat, QLhat)
{
ParamNames = this.ObsMatrix.Observation.ParamNames
};
if (!DoubleUtil.IsValid(s0))
{
log.Error("方差值无效！" + s0);
}
AdjustResultMatrix result = new AdjustResultMatrix()
.SetAdjustmentType(AdjustmentType.参数平差)
.SetEstimated(estX)
.SetCorrection(estV)
.SetCorrectedObs(correctedObs)
.SetCorrectedEstimate(CorrectedEstimate)
.SetObsMatrix(input)
.SetFreedom(freedom)
.SetVarianceFactor(s0)
.SetVtpv(vtpv);
return(result);
}
/// <summary>
/// 参数加权平差
/// </summary>
/// <param name="input"></param>
/// <returns></returns>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
//下标 o 表示观测值，x表示估计值，xa0表示具有先验信息的随机参数,
//xa为包含先验信息xa0的矩阵，是在整个误差方程中计算的矩阵
var Freedom = input.ObsCount - (input.ParamCount - input.Apriori.RowCount); // n -tb
Matrix A = new Matrix(input.Coefficient);
Matrix AT = A.Trans;
Matrix L = new Matrix((IMatrix)input.Observation);
Matrix Qo = new Matrix(input.Observation.InverseWeight);
Matrix Po = Qo.Inversion;
int obsCount = L.RowCount;
int paramCount = A.ColCount;
//具有先验信息的随机参数
Matrix Xa0 = new Matrix((IMatrix)input.Apriori);
Matrix Qxa0 = new Matrix(input.Apriori.InverseWeight);
Matrix Pxa0 = Qxa0.Inversion;
//计算先验信息的平差矩阵部分
Matrix Nxa0 = new Matrix(input.ParamCount);
Nxa0.SetSub(Pxa0);
Matrix Uxa0 = Pxa0 * Xa0;
Matrix Uxa = new Matrix(input.ParamCount, 1);
Uxa.SetSub(Uxa0);
//法方程系数阵
Matrix N = AT * Po * A + Nxa0;
Matrix InverN = N.Inversion;
//法方程右手边
Matrix U = AT * Po * L + Uxa;
//计算估值
Matrix X = InverN * U;
//精度估计
Matrix Xa = X.GetSub(0, 0, Xa0.RowCount);
Matrix dXa = Xa - Xa0;
Matrix V = A * X - L;
Matrix VT = V.Trans;
var vtpv = (VT * Po * V + dXa.Trans * Pxa0 * dXa).FirstValue;
var VarianceOfUnitWeight = vtpv / Freedom;
var Estimated = new WeightedVector(X, InverN)
{
ParamNames = input.ParamNames
};
AdjustResultMatrix result = new AdjustResultMatrix()
.SetEstimated(Estimated)
.SetObsMatrix(input)
.SetFreedom(Freedom)
.SetVarianceFactor(VarianceOfUnitWeight)
.SetVtpv(vtpv);
return(result);
}
/// <summary>
/// 数据处理。全部转化为计算偏移量，即，各种参数采用近似值，此处需要考虑初值情况。
/// </summary>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
//参数命名规则：下标 0 表示上一个，1 表示预测，无数字表示当次
#region 参数预测
WeightedVector apriori = input.Apriori;
if (apriori == null)
{
throw new ArgumentException("必须具有先验参数值。");
}
//先验值赋值
ArrayMatrix X0 = new ArrayMatrix(apriori); //上一次参数估值
ArrayMatrix Qx0 = new ArrayMatrix(apriori.InverseWeight.Array); //上一次估计误差方差权逆阵
ArrayMatrix Trans = new ArrayMatrix(input.Transfer.Array); //状态转移矩阵
ArrayMatrix TransT = Trans.Transpose();
ArrayMatrix Q_m = new ArrayMatrix(input.InverseWeightOfTransfer.Array); //状态转移模型噪声
//计算参数预测值，可以看做序贯平差中的第一组数据
ArrayMatrix X1 = Trans * X0;
ArrayMatrix Qx1 = Trans * Qx0 * TransT + Q_m;
var Predicted = new WeightedVector(X1, Qx1)
{
ParamNames = input.ParamNames
}; //结果为残差
#endregion
//System.IO.File.WriteAllText(saveDir + @"\Predicted.txt", Predicted.ToFormatedText());
// System.IO.File.WriteAllText(saveDir + @"\Apriori.txt", Apriori.ToFormatedText());
#region 参数估计
ArrayMatrix A = new ArrayMatrix(input.Coefficient.Array); //误差方程系数阵
ArrayMatrix AT = A.Transpose(); //A 的转置
//估计值才需要观测值，而预测值不需要
//观测值赋值
WeightedVector obs = input.Observation - input.FreeVector;
if (obs == null)
{
throw new ArgumentException("必须具有观测向量。");
}
ArrayMatrix L = new ArrayMatrix(obs); //观测值，或 观测值 - 估计值，！！
ArrayMatrix Q_o = new ArrayMatrix(obs.InverseWeight.Array); //观测噪声权逆阵
ArrayMatrix P_o = Q_o.Inverse;
//计算预测的观测残差 自由项
//由 V = A X - L, 得 V = A x - l, l = L - A X0, X = X0 + x
ArrayMatrix dL = L - A * X1;//此处注意符号
ArrayMatrix QdL = Q_o + A * Qx1 * AT;
//计算平差值的权阵
ArrayMatrix PXk = AT * P_o * A + Qx1.Inverse;
//计算平差值的权逆阵
ArrayMatrix Qx = PXk.Inverse;
//计算增益矩阵
ArrayMatrix J = Qx * AT * P_o;
//计算参数改正值和估值
ArrayMatrix deltaX = J * dL;
ArrayMatrix X = X1 + deltaX;//改 X0 为 X1
//精度估计
ArrayMatrix UnitMatrix = ArrayMatrix.EyeMatrix(J.RowCount, 1.0);
ArrayMatrix B = UnitMatrix - J * A;
Qx = B * Qx1 * B.Transposition + J * Q_o * J.Transposition; //参数权逆阵
// Matrix Qx = (AT * P_o * A + Qx1.Inverse).Inverse;
var Estimated = new WeightedVector(X, Qx)
{
ParamNames = input.ParamNames
};
#endregion
#region 验后观测残差
// Matrix Px = Qx.Inverse;
ArrayMatrix V = L - A * X;
this.PostfitObservation = new Vector(MatrixUtil.GetColVector(V.Array))
{
ParamNames = input.ParamNames
};
#endregion
#region 精度估计
this.SumOfObsCount += input.ObsCount;
var Freedom = SumOfObsCount - input.ParamCount;// input.Freedom;
//观测噪声权阵
ArrayMatrix V1TPV1 = deltaX.Transpose() * Qx1.Inverse * deltaX;
ArrayMatrix VTPV = V.Transpose() * P_o * V;
var vtpv = VTPV[0, 0];
double upper = (V1TPV1 + VTPV)[0, 0];
if (!DoubleUtil.IsValid(upper) || upper > 1e10 || upper < 0)
{
log.Debug("方差值无效！" + upper);
}
//赋值
var VarianceOfUnitWeight = Math.Abs(upper) / Freedom;
//System.IO.File.WriteAllText(saveDir + @"\Estimated.txt", Estimated.ToFormatedText());
//System.IO.File.WriteAllText(saveDir + @"\Observation.txt", Observation.ToFormatedText());
#endregion
AdjustResultMatrix result = new AdjustResultMatrix()
.SetEstimated(Estimated)
.SetFreedom(Freedom)
.SetObsMatrix(input)
.SetVarianceFactor(VarianceOfUnitWeight)
.SetVtpv(vtpv);
return(result);
}
/// <summary>
/// 计算
/// </summary>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
this.ObsMatrix = input;
//命名规则：0表示上一个（先验信息），1表示预测，无数字表示当次
//上次次观测设置
var Qx0 = new Matrix(input.Apriori.InverseWeight);
var Px0 = new Matrix(Qx0.GetInverse());
var X0 = new Matrix((IMatrix)input.Apriori);
//本次观测设置
var Qo = new Matrix(input.Observation.InverseWeight);
var Po = new Matrix(Qo.GetInverse());
var A = new Matrix(input.Coefficient);
var AT = A.Trans;
var L = new Matrix((IMatrix)input.Observation);
int obsCount = L.RowCount;
int paramCount = A.ColCount;
//具有状态转移的序贯平差
//if (input.Transfer != null && input.InverseWeightOfTransfer!=null)
//{
// Matrix Trans = new Matrix(input.Transfer.Array); //状态转移矩阵
// Matrix TransT = Trans.Transpose();
// Matrix Q_m = new Matrix(input.InverseWeightOfTransfer.Array); //状态转移模型噪声
// //计算参数预测值，可以看做序贯平差中的第一组数据
// //ArrayMatrix X1 = Trans * X0;
// //ArrayMatrix Qx1 = Trans * Qx0 * TransT + Q_m;
// X0 = Trans * X0;
// Qx0 = Trans * Qx0 * TransT + Q_m;
// Px0 = Qx0.Inversion;
//}
//1.预测残差
//计算预测残差
var V1 = L - A * X0; //观测值 - 估计近似值
var Qv1 = Qo + A * Qx0 * AT; //预测残差方差
//2.计算增益矩阵
var J = Qx0 * AT * Qv1.Inversion; // 增益矩阵
//3.平差结果
var dX = J * V1; //参数改正
var X = X0 + dX;
//4.精度评定
var Qx = Qx0 - J * A * Qx0;
var Estimated = new WeightedVector(X, Qx)
{
ParamNames = input.ParamNames
};
SumOfObsCount += input.ObsCount;
var Freedom = SumOfObsCount - input.ParamCount;// AprioriObsCount;
// var V = A * dX - V1;//估值-观测值 V = A * X - L = A * (X0 + deltaX) - (l + A * X0) = A * deltaX - l.
// this.VarianceOfUnitWeight = (V.Trans * Po * V).FirstValue / Freedom;//单位权方差
Matrix V = A * X - L;
Matrix Vx = X - X0;
Matrix VTPV = null;
if (Po.IsDiagonal)
{
VTPV = new Matrix(AdjustmentUtil.ATPA(V, Po)) + (Vx.Trans * Px0 * Vx);
}
else
{
VTPV = V.Trans * Po * V + (Vx.Trans * Px0 * Vx);
}
var vtpv = VTPV[0, 0];
this.SumOfVptv += vtpv;
this.SumOfObsCount += A.RowCount;
var VarianceOfUnitWeight = Math.Abs(vtpv / (Freedom));
AdjustResultMatrix result = new AdjustResultMatrix()
.SetEstimated(Estimated)
.SetFreedom(Freedom)
.SetObsMatrix(input)
.SetVarianceFactor(VarianceOfUnitWeight)
.SetVtpv(vtpv);
return(result);
}
/// <summary>
/// 构建不变参数的计算结果。
/// 注意：需要控制参数的增减问题。
/// 基本思路：增加则插入，减少则删除，通过参数名称来控制。
/// </summary>
/// <param name="obsMatrix"></param>
/// <param name="NormalEquationSuperposer"></param>
/// <returns></returns>
public static AdjustResultMatrix GetConstParamResult(AdjustObsMatrix obsMatrix, NormalEquationSuperposer NormalEquationSuperposer)
{
//观测值权阵设置,对已知量赋值
Matrix L = new Matrix((IMatrix)obsMatrix.Observation);
Matrix QL = new Matrix(obsMatrix.Observation.InverseWeight);
Matrix PL = new Matrix(QL.GetInverse());
Matrix A = new Matrix(obsMatrix.Coefficient);
Matrix AT = A.Trans;
Matrix B = new Matrix(obsMatrix.SecondCoefficient);
Matrix BT = B.Trans;
Matrix X0 = obsMatrix.HasApprox ? new Matrix(obsMatrix.ApproxVector, true) : null;
Matrix Y0 = obsMatrix.HasSecondApprox ? new Matrix(obsMatrix.SecondApproxVector, true) : null;
Matrix D = obsMatrix.HasFreeVector ? new Matrix(obsMatrix.FreeVector, true) : null;
int obsCount = L.RowCount;
int fixedParamCount = obsMatrix.SecondParamNames.Count;// B.ColCount;
int freedom = obsCount - fixedParamCount;
//观测值更新
Matrix lxy = L - (A * X0 + B * Y0 + D); //采用估值计算的观测值小量
Matrix ATPL = AT * PL;
//法方程
Matrix Na = ATPL * A;
Matrix Nab = AT * PL * B;
Matrix InverNa = Na.Inversion;
Matrix J = A * InverNa * AT * PL;
Matrix I = Matrix.CreateIdentity(J.RowCount);
Matrix B2 = (I - J) * B; //新的系数阵 Ac, 原文中为 B波浪~
Matrix B2T = B2.Trans;
Matrix B2TPL = B2T * PL;
Matrix NofB2 = B2TPL * B2;
Matrix UofB2 = B2TPL * lxy;
NofB2.ColNames = obsMatrix.SecondParamNames;
NofB2.RowNames = obsMatrix.SecondParamNames;
UofB2.RowNames = obsMatrix.SecondParamNames;
UofB2.ColNames = new List <string>()
{
"ConstParam"
};
//生成法方程
var ne = new MatrixEquation(NofB2, UofB2);
//叠加法方程
NormalEquationSuperposer.Add(ne);//添加到法方程迭加器中
var acNe = NormalEquationSuperposer.CurrentAccumulated;
Matrix inverN = acNe.N.Inversion;
Matrix y = inverN * acNe.U;
y.RowNames = acNe.ParamNames;
Matrix Qy = inverN;
Qy.ColNames = acNe.ParamNames;
Qy.RowNames = acNe.ParamNames;
var estY = new WeightedVector(y, Qy)
{
ParamNames = acNe.ParamNames
};
var V = B2 * y - lxy;
Matrix Qv = QL - B2 * Qy * B2T;
Matrix Y = Y0 + y;
var vtpv = (V.Trans * PL * V).FirstValue;
double s0 = vtpv / (freedom == 0 ? 0.1 : freedom);//单位权方差
WeightedVector CorrectedEstimate = new WeightedVector(Y, Qy);
WeightedVector estV = new WeightedVector(V, Qv)
{
ParamNames = obsMatrix.Observation.ParamNames
};
Matrix Lhat = L + V;
Matrix QLhat = B2 * Qy * B2T;
var correctedObs = new WeightedVector(Lhat, QLhat)
{
ParamNames = obsMatrix.Observation.ParamNames
};
AdjustResultMatrix result = new AdjustResultMatrix()
.SetAdjustmentType(AdjustmentType.递归最小二乘)
.SetEstimated(estY)
.SetCorrection(estV)
.SetCorrectedObs(correctedObs)
.SetCorrectedEstimate(CorrectedEstimate)
.SetObsMatrix(obsMatrix)
.SetFreedom(freedom)
.SetVarianceFactor(s0)
.SetVtpv(vtpv);
return(result);
}
/// <summary>
/// 第三步：计算易变参数
/// </summary>
/// <param name="estY"></param>
/// <param name="obsMatrix"></param>
/// <returns></returns>
private AdjustResultMatrix Step3GetMutableX(WeightedVector estY, AdjustObsMatrix obsMatrix)
{
Matrix constY = estY.GetVectorMatrix();
Matrix constQy = estY.InverseWeight;
//观测值权阵设置,对已知量赋值
Matrix L = new Matrix((IMatrix)obsMatrix.Observation);
Matrix QL = new Matrix(obsMatrix.Observation.InverseWeight);
Matrix PL = new Matrix(QL.GetInverse());
Matrix A = new Matrix(obsMatrix.Coefficient);
Matrix AT = A.Trans;
Matrix B = new Matrix(obsMatrix.SecondCoefficient);
Matrix BT = B.Trans;
Matrix X0 = obsMatrix.HasApprox ? new Matrix(obsMatrix.ApproxVector, true) : null;
Matrix Y0 = obsMatrix.HasSecondApprox ? new Matrix(obsMatrix.SecondApproxVector, true) : null;
Matrix D = obsMatrix.HasFreeVector ? new Matrix(obsMatrix.FreeVector, true) : null;
int obsCount = L.RowCount;
int fixedParamCount = B.ColCount;
int mutableParamCount = A.ColCount;
int paramCount = fixedParamCount + mutableParamCount;
int freedom = obsCount - paramCount;
Matrix lxy = L - (A * X0 + B * Y0 + D); //采用估值计算的观测值小量
Matrix ATPL = AT * PL;
//法方程
Matrix Na = ATPL * A;
Matrix Nab = AT * PL * B;
Matrix InverNa = Na.Inversion;
//求x
//观测值更新,采用估值进行计算
Matrix lx = lxy - B * constY; // = L - (A * X0 + B * Y + D);
/* Matrix x = InverNa * ATPL * lx;*/
Matrix x = InverNa * (ATPL * lxy - Nab * constY); //这两个计算是等价的
Matrix X = X0 + x;
Matrix Ntmp = Na.Inversion * Nab;
Matrix Qx = InverNa + Ntmp * constQy * Ntmp.Trans;
//Matrix Qxy = AT * PL * B;
//Matrix Qtemp = Qx * Qxy;
//Matrix Dx = Qx + Qtemp * Dy * Qtemp.Trans;
//精度评定
Matrix V = A * x - lx;
Matrix Qv = QL - A * Qx * AT - B * constQy * BT;
// Matrix lT = l.Trans;
double vtpv = (V.Trans * PL * V).FirstValue; //(lT * PL * l - lT * PL * Ac * y).FirstValue;//
double s0 = vtpv / (freedom == 0 ? 0.1 : freedom); //单位权方差
WeightedVector estX = new WeightedVector(x, Qx)
{
ParamNames = obsMatrix.ParamNames
};
WeightedVector CorrectedEstimate = new WeightedVector(X, Qx)
{
ParamNames = obsMatrix.ParamNames
};
WeightedVector estV = new WeightedVector(V, Qv)
{
ParamNames = obsMatrix.Observation.ParamNames
};
Matrix Lhat = L + V;
Matrix QLhat = A * Qx * AT;
var correctedObs = new WeightedVector(Lhat, QLhat)
{
ParamNames = this.ObsMatrix.Observation.ParamNames
};
if (!DoubleUtil.IsValid(s0))
{
log.Error("方差值无效！" + s0);
}
AdjustResultMatrix result = new AdjustResultMatrix()
.SetAdjustmentType(AdjustmentType.递归最小二乘)
.SetEstimated(estX)
.SetSecondEstimated(estY)
.SetCorrection(estV)
.SetCorrectedObs(correctedObs)
.SetCorrectedEstimate(CorrectedEstimate)
.SetObsMatrix(obsMatrix)
.SetFreedom(freedom)
.SetVarianceFactor(s0)
.SetVtpv(vtpv);
return(result);
}
/// <summary>
/// 计算
/// </summary>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
this.ObsMatrix = input;
//命名规则：0表示上一个（先验信息），1表示预测，无数字表示当次
//上次次观测设置
var Qx0 = new Matrix(input.Apriori.InverseWeight);
var Px0 = new Matrix(Qx0.GetInverse());
var X0 = new Matrix((IMatrix)input.Apriori);
//本次观测设置
var Qo = new Matrix(input.Observation.InverseWeight);
var Po = new Matrix(Qo.GetInverse());
var A = new Matrix(input.Coefficient);
var AT = A.Trans;
var L = new Matrix((IMatrix)input.Observation);
int obsCount = L.RowCount;
int paramCount = A.ColCount;
//具有状态转移的序贯平差
if (input.Transfer != null && input.InverseWeightOfTransfer != null)
{
Matrix Trans = new Matrix(input.Transfer.Array); //状态转移矩阵
Matrix TransT = Trans.Transpose();
Matrix Q_m = new Matrix(input.InverseWeightOfTransfer.Array); //状态转移模型噪声
//计算参数预测值，可以看做序贯平差中的第一组数据
//ArrayMatrix X1 = Trans * X0;
//ArrayMatrix Qx1 = Trans * Qx0 * TransT + Q_m;
//更新先验值
X0 = Trans * X0;
Qx0 = Trans * Qx0 * TransT + Q_m;
Px0 = Qx0.Inversion;
}
var ATP = AT * Po;
var N = ATP * A;
var U = ATP * L;
var Px = (N + Px0);
var Qx = Px.Inversion;
var X = Qx * (U + Px0 * X0);
var Estimated = new WeightedVector(X, Qx)
{
ParamNames = input.ParamNames
};
SumOfObsCount += input.ObsCount;
var Freedom = SumOfObsCount - input.ParamCount;// AprioriObsCount;
Matrix V = A * X - L;
Matrix Vx = X - X0;
Matrix VTPV = null;
if (Po.IsDiagonal)
{
VTPV = new Matrix(AdjustmentUtil.ATPA(V, Po)) + (Vx.Trans * Px0 * Vx);
}
else
{
VTPV = V.Trans * Po * V + (Vx.Trans * Px0 * Vx);
}
var vtpv = VTPV[0, 0];
this.SumOfObsCount += A.RowCount;
var VarianceOfUnitWeight = Math.Abs(vtpv / (Freedom));
AdjustResultMatrix result = new AdjustResultMatrix()
.SetEstimated(Estimated)
.SetFreedom(Freedom)
.SetObsMatrix(input)
.SetVarianceFactor(VarianceOfUnitWeight)
.SetVtpv(vtpv);
return(result);
}
/// <summary>
/// 数据计算
/// </summary>
/// <param name="input">观测矩阵</param>
/// <returns></returns>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
this.ObsMatrix = input;
//观测值权阵设置,对已知量赋值
Matrix L = new Matrix((IMatrix)input.Observation);
Matrix QL = new Matrix(input.Observation.InverseWeight);
Matrix PL = new Matrix(QL.GetInverse());
Matrix A = new Matrix(input.Coefficient);
Matrix AT = A.Trans;
Matrix B = new Matrix(input.SecondCoefficient);
Matrix BT = B.Trans;
Matrix X0 = input.HasApprox ? new Matrix(input.ApproxVector, true) : null;
Matrix Y0 = input.HasSecondApprox ? new Matrix(input.SecondApproxVector, true) : null;
Matrix D = input.HasFreeVector ? new Matrix(input.FreeVector, true) : null;
int obsCount = L.RowCount;
int fixedParamCount = B.ColCount;
//观测值更新
Matrix l = L - (A * X0 + B * Y0 + D); //如果null，则是本身
Matrix ATPL = AT * PL;
//法方程
Matrix Na = ATPL * A;
Matrix Nab = AT * PL * B;
Matrix InverNa = Na.Inversion;
Matrix J = A * InverNa * AT * PL;
Matrix B2 = (Matrix.CreateIdentity(J.RowCount) - J) * B;
Matrix AcT = B2.Trans;
Matrix Nc = AcT * PL * B2;;
Matrix InverNc = Nc.Inversion;
//平差结果
Matrix y = InverNc * AcT * PL * l;
Matrix Y = Y0 + y;
Matrix Qy = InverNc;
//只针对y的精度评定
Matrix W = B2 * y - l;
double ys0 = (W.Trans * PL * W).FirstValue / (obsCount - fixedParamCount);
Matrix Dy = ys0 * Qy;
//求x
Matrix lx = L - (A * X0 + B * Y + D); //如果null，则是本身
Matrix x = InverNa * ATPL * lx; //这两个计算是等价的
// Matrix x = InverNa * (ATPL * l - Nab * y);//这两个计算是等价的
Matrix X = X0 + x;
Matrix Ntmp = Na.Inversion * Nab;
Matrix Qx = InverNa + Ntmp * Qy * Ntmp.Trans;
//Matrix Qxy = AT * PL * B;
//Matrix Qtemp = Qx * Qxy;
//Matrix QtempT = Qtemp.Trans;
//Matrix Dx = Qx + Qtemp * Dy * QtempT;
//精度评定
Matrix V = A * x + B * y - l;
Matrix Qv = QL - A * Qx * AT - B * Qy * BT;
// Matrix lT = l.Trans;
double vtpv = (V.Trans * PL * V)[0, 0];//(lT * PL * l - lT * PL * Ac * y).FirstValue;//
int paramCount = A.ColCount + B.ColCount;
int freedom = A.RowCount - paramCount;
double s0 = vtpv / (freedom == 0 ? 0.1 : freedom);//单位权方差
WeightedVector estX = new WeightedVector(x, Qx)
{
ParamNames = input.ParamNames
};
WeightedVector CorrectedEstimate = new WeightedVector(X, Qx)
{
ParamNames = input.ParamNames
};
WeightedVector estY = new WeightedVector(y, Qy)
{
ParamNames = input.SecondParamNames
};
WeightedVector estV = new WeightedVector(V, Qv);
Matrix Lhat = L + V;
Matrix QLhat = A * Qx * AT;
var correctedObs = new WeightedVector(Lhat, QLhat)
{
ParamNames = this.ObsMatrix.Observation.ParamNames
};
if (!DoubleUtil.IsValid(s0))
{
log.Error("方差值无效！" + s0);
}
AdjustResultMatrix result = new AdjustResultMatrix()
.SetAdjustmentType(AdjustmentType.单期递归最小二乘)
.SetEstimated(estX)
.SetSecondEstimated(estY)
.SetCorrection(estV)
.SetCorrectedObs(correctedObs)
.SetCorrectedEstimate(CorrectedEstimate)
.SetObsMatrix(input)
.SetFreedom(freedom)
.SetVarianceFactor(s0)
.SetVtpv(vtpv);
return(result);
}
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
//u(0<u<t)个独立量为参数,多余观测数r=n-t
//原始输入 m=r+u
Matrix A = input.Coefficient; //m×t
Matrix B = input.SecondCoefficient; //m×n
Matrix L = new Matrix((IMatrix)input.Observation); //n×1
Matrix QL = new Matrix((IMatrix)input.Observation.InverseWeight); //m×m
Matrix X0 = input.HasApprox ? new Matrix(input.ApproxVector, true) : null; //u×1
Matrix D = input.HasFreeVector ? new Matrix(input.FreeVector, true) : null; //B0//m×1
Matrix B0 = input.HasSecondFreeVector ? new Matrix(input.SecondFreeVector, true) : null; //B0//m×1
Matrix AT = A.Trans;
Matrix PL = QL.Inversion;
Matrix BT = B.Trans;
//多余观测数为r=m-u=n-t
int obsCount = L.RowCount;
int totalConditionCount = B.RowCount;
int paramCount = A.ColCount;//u
int freedom = obsCount - paramCount;
//观测值更新
Matrix l = L - (A * X0 + D); //如果null，则是本身
Matrix W = -(B * X0 - B0);
Matrix N = AT * PL * A;
Matrix U = AT * PL * l;
Matrix inverN = N.Inversion;
// ************************
//采用分块矩阵直接计算
int dimOfX = N.ColCount; //X的维度
int dimOfK = B.RowCount; //K的维度
int dimOfBigN = dimOfK + dimOfX;
Matrix BigN = new Matrix(dimOfBigN, dimOfBigN);
BigN.SetSub(N);
BigN.SetSub(BT, 0, dimOfX);
BigN.SetSub(B, dimOfX, 0);
Matrix inverBigN = BigN.Inversion;
Matrix bigW = new Matrix(dimOfBigN, 1);
bigW.SetSub(U);
bigW.SetSub(W, U.RowCount);
Matrix bigX = inverBigN * bigW;
Matrix x = bigX.GetSub(0, 0, dimOfX);
Matrix K = bigX.GetSub(dimOfX, 0);
Matrix X = X0 + x;
Matrix V = A * x - l;
Matrix EstL = L + V;
//**************精度估计---------------
Matrix Q11 = inverBigN.GetSub(0, 0, dimOfX, dimOfX);
Matrix Q12 = inverBigN.GetSub(0, dimOfX, dimOfK);
Matrix Q21 = inverBigN.GetSub(dimOfX, 0, dimOfK, dimOfK);
Matrix Q22 = inverBigN.GetSub(dimOfX, dimOfX);
Matrix Qx = Q11;
Matrix QestL = A * Q11 * AT;
var coWeightedX = new WeightedVector(X, Qx);
var weightedX = new WeightedVector(x, Qx);
var weightedL = new WeightedVector(EstL, QestL);
double vtpv = (V.Trans * PL * V).FirstValue;
double s0 = vtpv / freedom;//单位权中误差估值
if (!DoubleUtil.IsValid(s0))
{
log.Error("方差值无效！" + s0);
}
AdjustResultMatrix result = new AdjustResultMatrix()
.SetAdjustmentType(AdjustmentType.具有参数的条件平差)
.SetEstimated(weightedX)
.SetCorrectedEstimate(coWeightedX)
.SetCorrectedObs(weightedL)
.SetObsMatrix(input)
.SetFreedom(freedom)
.SetVarianceFactor(s0)
.SetVtpv(vtpv);
return(result);
}
/// <summary>
/// 构造函数
/// </summary>
/// <param name="Adjustment">平差结果</param>
/// <param name="material">原材料</param>
public AdjustmentResult(AdjustResultMatrix Adjustment, TMaterial material)
{
this.ResultMatrix = Adjustment;
this.Material = material;
}
/// <summary>
/// 一维滤波矩阵构造器
/// </summary>
/// <param name="obs"></param>
/// <param name="PrevAdjustment"></param>
public OneDimAdjustMatrixBuilder(double obs, AdjustResultMatrix PrevAdjustment = null, double rmsOrStdDev = 1)
: this(new RmsedNumeral(obs, rmsOrStdDev))
{
}
