/// <summary>
/// Kalman滤波计算不变参数。
/// </summary>
/// <param name="input"></param>
/// <returns></returns>
private AdjustResultMatrix GetSequentialConst(AdjustObsMatrix input)
{
WeightedVector appri = null;
Matrix B2 = input.BuildCoeefOfConstParam();
if (LastConstResult != null)
{
var IsEqual = Geo.Utils.ListUtil.IsEqual(LastConstResult.ParamNames, input.SecondParamNames);
if (IsEqual)
{
appri = LastConstResult.Estimated;
}
else
{
appri = SimpleAdjustMatrixBuilder.GetNewWeighedVectorInOrder(input.SecondParamNames, LastConstResult.Estimated);
}
}
if (appri == null)//第一次，使用参数平差结果
{
ParamAdjuster paramAdjuster = new ParamAdjuster();
var paramResult = paramAdjuster.Run(new AdjustObsMatrix(input.Observation, B2, null, input.SecondParamNames));
appri = paramResult.Estimated;
}
AdjustObsMatrix obsMatrix1 = new AdjustObsMatrix(appri, input.Observation, B2, input.SecondTransfer);
obsMatrix1.ParamNames = input.SecondParamNames;
//var kalmanFilter = new SimpleKalmanFilter();
var res = MatrixAdjuster.Run(obsMatrix1);
return(res);
}
/// <summary>
/// 建立固定参数与浮点解转换的系数阵.
/// 这是直接对等的做法。
/// </summary>
/// <param name="totalFloat"></param>
/// <param name="fixedAmbiguities"></param>
/// <returns></returns>
private Matrix BuildCoeefOfFixedToFloat(WeightedVector totalFloat, WeightedVector fixedAmbiguities)
{
//首先外部优先
if (CoeefOfFixedToFloatBuildEventHandler != null)
{
return(CoeefOfFixedToFloatBuildEventHandler(totalFloat, fixedAmbiguities));
}
//外部没有则，直接点对点
int totalParamCount = totalFloat.Count; //待估参数个数
int fixedAmbiCount = fixedAmbiguities.Count;
var coeefOfFixedToFloat = new Matrix(fixedAmbiCount, totalParamCount)
{
RowNames = fixedAmbiguities.ParamNames
};
//constraints to fixed ambiguities
for (int i = 0; i < fixedAmbiCount; i++)
{
//对所有的参数
var name = fixedAmbiguities.ParamNames[i];
int j = totalFloat.ParamNames.IndexOf(name);
coeefOfFixedToFloat[i, j] = 1;
}
return(coeefOfFixedToFloat);
}
public static AdjustObsMatrix BuildATtest(int paramCount, int obsCount, AdjustmentType adjustmentType = AdjustmentType.参数平差)
{
bool isParamOrCondition = adjustmentType == AdjustmentType.参数平差;
int restCount = obsCount - paramCount;
int rowOfCoeef = isParamOrCondition ? obsCount : restCount;
int colOfCoeef = isParamOrCondition ? paramCount : obsCount;
AdjustObsMatrix obsMatrix = new AdjustObsMatrix();
obsMatrix.Observation = WeightedVector.GenerateATest(obsCount);
obsMatrix.Apriori = WeightedVector.GenerateATest(paramCount);
obsMatrix.Coefficient = BuildACoeefient(rowOfCoeef, colOfCoeef);
obsMatrix.Transfer = new WeightedMatrix(Matrix.CreateIdentity(paramCount), Matrix.CreateIdentity(paramCount));
obsMatrix.FreeVector = new Vector(rowOfCoeef, 1);
obsMatrix.SecondFreeVector = new Vector(paramCount, 1);
obsMatrix.ApproxVector = new Vector(paramCount, 1);
obsMatrix.SecondApproxVector = new Vector(paramCount, 1);
if (adjustmentType == AdjustmentType.具有参数的条件平差)
{
obsMatrix.SecondCoefficient = BuildACoeefient(rowOfCoeef, colOfCoeef);
}
return(obsMatrix);
}
private static WeightedVector GetConstY(List <AdjustObsMatrix> inputs)
{
var normals = Setp1ComposeConstNormaEqualtion(inputs);
WeightedVector estY = Step2GetConstY(normals);
return(estY);
}
/// <summary>
/// 参数平差构造函数。
/// </summary>
/// <param name="observation">观测值和协方差</param>
/// <param name="coeffOfParams">系数阵</param>
/// <param name="approx">是否使用参数的偏移量</param>
/// <param name="paramNames">参数名称</param>
public AdjustObsMatrix(WeightedVector observation, Matrix coeffOfParams, Vector approx = null, List <string> paramNames = null)
{
this.Coefficient = (coeffOfParams);
this.Observation = observation;
this.ApproxVector = approx;
if (paramNames != null)
{
this.ParamNames = new List <string>(paramNames);
}
}
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="fixedAmbiguities">已经固定的模糊度</param>
/// <param name="totalFloat">浮点解平差信息</param>
/// <returns></returns>
public WeightedVector GetResultByConditionAdjust(WeightedVector totalFloat, WeightedVector fixedAmbiguities)
{
if (fixedAmbiguities.Count == 0)
{
return(totalFloat);
}
//新增加约束条件的系数矩阵和权矩阵、观测值
Matrix coeefOfCondition = BuildCoeefOfFixedToFloat(totalFloat, fixedAmbiguities);
WeightedVector NewEstimated = SolveAmbiFixedResultByConditionAdjust(totalFloat, fixedAmbiguities, coeefOfCondition);
return(NewEstimated);
}
/// <summary>
/// 获取固定模糊度后的结果。默认参数直接固定，并通过名称关联。
/// </summary>
/// <param name="fixedAmbiguities"></param>
/// <param name="originalVector"></param>
/// <returns></returns>
public WeightedVector GetParamFixedResult(WeightedVector originalVector, WeightedVector fixedAmbiguities)
{
WeightedVector result = null;
if (IsFixParamByConditionOrHugeWeight)
{
result = GetResultByConditionAdjust(originalVector, fixedAmbiguities);
}
else
{
result = GetResultByWeighedParamAdjust(originalVector, fixedAmbiguities);
}
return(result);
}
/// <summary>
/// kalman滤波构造函数。
/// </summary>
/// <param name="coeff">A n*m阶设计矩阵，也称观测矩阵，系数阵</param>
/// <param name="apriori">先验值</param>
/// <param name="observation">观测值 L,满足 l = L - (AX0 + D), 若X0与D为null， 则l=L </param>
/// <param name="trans">Φ 状态转移矩阵和Q_m 动力模型噪声向量 W 的方差</param>
/// <param name="approx">参数近似向量X0,满足 l = L - (AX0 + D) </param>
/// <param name="freeVector">常数项D，满足 l = L - (AX0 + D) </param>
public AdjustObsMatrix(
WeightedVector apriori,
WeightedVector observation,
Matrix coeff,
WeightedMatrix trans,
Vector approx = null,
Vector freeVector = null
)
{
this.Coefficient = coeff;
this.Apriori = apriori;
this.Observation = observation;
this.Transfer = trans;
this.ApproxVector = approx;
this.FreeVector = freeVector;
}
/// <summary>
/// 估计，改正。一个更健壮的方法。
/// </summary>
/// <param name="observation">观测值信息</param>
/// <param name="control">控制矩阵，有时为非对称阵，如PPP</param>
/// <param name="covaOfObs">观测值协方差</param>
private WeightedVector CorrectSimple(IMatrix observation, IMatrix control, IMatrix covaOfObs)
{
//简化字母表示
Matrix Q1 = new Matrix(CovaOfPredictParam);
Matrix P1 = new Matrix(CovaOfPredictParam.GetInverse());
Matrix X1 = new Matrix(PredictParam);
Matrix L = new Matrix(observation);
Matrix A = new Matrix(control);
Matrix AT = new Matrix(A.Transposition);
Matrix Po = new Matrix(covaOfObs.GetInverse());
Matrix Qo = new Matrix(covaOfObs);
//平差值Xk的权阵
Matrix PXk = null;
Matrix Atpa = null;
if (Po.IsDiagonal)
{
Atpa = ATPA(A, Po);
}
else
{
Atpa = AT * Po * A;
}
PXk = new Matrix(SymmetricMatrix.Parse(Atpa)) + P1;
//计算平差值的权逆阵
Matrix Qx = PXk.Inversion;
Matrix J = Qx * AT * Po;
//计算平差值
Matrix Vk1 = A * X1 - L;//计算新息向量
Matrix X = X1 - J * Vk1;
X.RowNames = ObsMatrix.ParamNames;
BuildCovaFactor(L, A, Po, P1, X1, X);
var Estimated = new WeightedVector(X, Qx)
{
ParamNames = ObsMatrix.ParamNames
};
return(Estimated);
}
/// <summary>
/// 解析为对象
/// </summary>
/// <param name="splitterOfMatrixItems"></param>
/// <param name="obs"></param>
/// <param name="currentBlockSb"></param>
/// <param name="currentType"></param>
private static void ParseBufferText(string[] splitterOfMatrixItems, AdjustObsMatrix obs, StringBuilder currentBlockSb, ObsMatrixType currentType)
{
switch (currentType)
{
case ObsMatrixType.Apriori:
obs.Apriori = WeightedVector.Parse(currentBlockSb.ToString());
break;
case ObsMatrixType.Observation:
obs.Observation = WeightedVector.Parse(currentBlockSb.ToString(), splitterOfMatrixItems);
break;
case ObsMatrixType.Coefficient:
obs.Coefficient = Matrix.Parse(currentBlockSb.ToString(), splitterOfMatrixItems);
break;
case ObsMatrixType.SecondCoefficient:
obs.SecondCoefficient = Matrix.Parse(currentBlockSb.ToString(), splitterOfMatrixItems);
break;
case ObsMatrixType.Transfer:
obs.Transfer = WeightedMatrix.Parse(currentBlockSb.ToString(), splitterOfMatrixItems);
break;
case ObsMatrixType.ApproxVector:
obs.ApproxVector = Vector.Parse(currentBlockSb.ToString());
break;
case ObsMatrixType.SecondApproxVector:
obs.SecondApproxVector = Vector.Parse(currentBlockSb.ToString());
break;
case ObsMatrixType.FreeVector:
obs.FreeVector = Vector.Parse(currentBlockSb.ToString());
break;
case ObsMatrixType.SecondFreeVector:
obs.SecondFreeVector = Vector.Parse(currentBlockSb.ToString());
break;
default:
break;
}
currentBlockSb.Clear();
}
/// <summary>
/// 条件平差法解算固定解，将固定解当成虚拟观测量,对原浮点解进行约束，条件平差。
/// </summary>
/// <param name="coeffOfParam">系数阵,条件方程构造</param>
/// <param name="totalFloat">原浮点解</param>
/// <param name="fixedObs">已经固定的参数，固定解当成虚拟观测量</param>
/// <returns></returns>
public static WeightedVector SolveAmbiFixedResultByConditionAdjust(WeightedVector totalFloat, Vector fixedObs, IMatrix coeffOfParam)
{
//以下算法已经验证等价！！2018.09.02， czs, hmx
bool isSong = false;
WeightedVector NewEstimated = null;
if (isSong)
{
#region 求固定解 宋力杰方法
IMatrix X_old = totalFloat;
IMatrix QX_old = totalFloat.InverseWeight;
IMatrix coeffOfParamT = coeffOfParam.Transposition;
IMatrix W = coeffOfParam.Multiply(X_old).Minus(new VectorMatrix(fixedObs));
IMatrix tmp = coeffOfParam.Multiply(QX_old).Multiply(coeffOfParamT);
IMatrix Nadd = (QX_old.Multiply(coeffOfParamT)).Multiply(tmp.GetInverse());
IMatrix X_new = X_old.Minus(Nadd.Multiply(W));
IMatrix tmp2 = Nadd.Multiply(coeffOfParam);
IMatrix QX_new = QX_old.Minus(tmp2.Multiply(QX_old));
NewEstimated = new WeightedVector(X_new, QX_new)
{
ParamNames = coeffOfParam.ColNames
};
#endregion
}
else
{
//条件平差
AdjustObsMatrix obsMatrix = new AdjustObsMatrix();
obsMatrix.SetCoefficient(coeffOfParam).SetObservation(totalFloat).SetFreeVector(fixedObs);
ConditionalAdjuster adjuster = new ConditionalAdjuster();
var resultMatrix = adjuster.Run(obsMatrix);
NewEstimated = resultMatrix.CorrectedObs;
}
return(NewEstimated);
}
/// <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>
/// </summary>
/// <param name="observation">观测值信息</param>
/// <param name="control">控制矩阵，有时为非对称阵，如PPP</param>
/// <param name="covaOfObs">观测值协方差</param>
private WeightedVector CorrectNormal(IMatrix observation, IMatrix control, IMatrix covaOfObs)
{
//简化字母表示
Matrix Q1 = new Matrix(CovaOfPredictParam);
Matrix P1 = new Matrix(CovaOfPredictParam.GetInverse());
Matrix X1 = new Matrix(PredictParam);
Matrix L = new Matrix(observation);
Matrix A = new Matrix(control);
Matrix AT = new Matrix(A.Transposition);
Matrix Po = new Matrix(covaOfObs.GetInverse());
Matrix Qo = new Matrix(covaOfObs);
/******* Normal method Start ********/
//计算增益矩阵
var temp = (Qo + (A * Q1 * AT)).Inversion; // (Qo.Plus(A.Multiply(Q1).Multiply(AT))).GetInverse();
var J = Q1 * AT * temp; //Q1.Multiply(AT).Multiply(temp);
//计算估计值
Matrix Vk1 = A * X1 - L; //计算新息向量A.Multiply(X1).Minus(L);
Matrix X = X1 - J * Vk1; //X1.Minus(J.Multiply(Vk1));
//计算平差值的权逆阵
var JA = J * A; // J.Multiply(A);
var temp2 = JA * Q1; //JA.Multiply(Q1);
var Qx = Q1 - temp2; //Q1.Minus(temp2);
//var maxtrix = new Matrix(Q1.Array) - new Matrix(temp2.Array);
//var differ2 = maxtrix.Minus(Qx);
//var I = DiagonalMatrix.GetIdentity(Q1.ColCount);
//var Qx2 = I.Minus(JA).Multiply(Q1);
//var differ = Qx2 .Minus(Qx);
/******* Normal method End ********/
var Estimated = new WeightedVector(X, Qx)
{
ParamNames = ObsMatrix.ParamNames
};
//观测残差
BuildCovaFactor(L, A, Po, P1, X1, X);
return(Estimated);
}
/// <summary>
/// 计算
/// </summary>
/// <param name="filePath"></param>
public override void Run(string filePath)
{
ResultTables.Clear();
IsCancel = false;
ObjectTableManagerReader reader = new ObjectTableManagerReader(filePath);
ObjectTableManager tables = reader.Read();
var approxTable = tables.Get(AdjustName.Approx);
var paramNameTable = tables.Get(AdjustName.ParamName);
var obsTable = tables.Get(AdjustName.Obs);
var rmsOfObsTable = tables.Get(AdjustName.RmsOfObs);
var designTable = tables.Get(AdjustName.Design);
var transTable = tables.Get(AdjustName.Trans);
var rmsOfTransTable = tables.Get(AdjustName.RmsOfTrans);
var aprioriTable = tables.Get(AdjustName.Apriori);
var rmsOfAprioriTable = tables.Get(AdjustName.RmsOfApriori);
int length = obsTable.RowCount;
InitProcess(length);
Vector approx = null;
if (approxTable != null && approxTable.RowCount > 0)
{
approx = new Vector(approxTable.BufferedValues[0]);
}
WeightedVector apriori = null;
if (aprioriTable != null && aprioriTable.RowCount > 0 && rmsOfAprioriTable != null && rmsOfAprioriTable.RowCount > 0)
{
apriori = new WeightedVector(new Vector(aprioriTable.BufferedValues[0]),
new Matrix(new Vector(rmsOfAprioriTable.BufferedValues[0])));
}
for (int i = 0; i < length; i++)
{
if (IsCancel)
{
break;
}
Dictionary <string, Object> obsRow = obsTable.BufferedValues[i];
Dictionary <string, Object> rmsOfObsRow = rmsOfObsTable.BufferedValues[i];
Dictionary <string, Object> designRow = designTable.BufferedValues[i];
Dictionary <string, Object> paramNameRow = paramNameTable.BufferedValues[i];
Vector obs = new Vector(obsRow);
Vector RmsOfObs = new Vector(rmsOfObsRow);
Vector Design = new Vector(designRow);
int obsCount = obsRow.Count;
int paramCount = paramNameRow.Count;
Matrix RmsOfObsMatrix = new Matrix(RmsOfObs);
Matrix designMatrix = new Matrix(Design.OneDimArray, obsCount, paramCount);
List <string> paramNames = new List <string>();
foreach (var item in paramNameRow)
{
paramNames.Add(item.Key);
} //平差矩阵生成
ManualAdjustMatrixBuilder matrixBuilder = new ManualAdjustMatrixBuilder();
matrixBuilder.ApproxParam = approx;
matrixBuilder.SetCoeffOfDesign(designMatrix)
.SetObsMinusApprox(new WeightedVector(obs, RmsOfObsMatrix.Pow(2.0)))
.SetParamNames(paramNames);
#region 先验值
if (apriori == null)
{
if (CurrentResult == null)
{
apriori = CreateInitAprioriParam(paramCount);
apriori.ParamNames = paramNames;
}
else if (!IsParamsChanged(paramNames))
{
apriori = CurrentResult.ResultMatrix.Estimated;
}
else
{
apriori = SimpleAdjustMatrixBuilder.GetNewWeighedVectorInOrder(paramNames, CurrentResult.ResultMatrix.Estimated);
}
}
matrixBuilder.SetAprioriParam(apriori);
apriori = null;
#endregion
//非必须的转移矩阵
if ((transTable != null && transTable.BufferedValues.Count > i) &&
(rmsOfTransTable != null && rmsOfTransTable.BufferedValues.Count > i))
{
var transRow = transTable.BufferedValues[i];
Vector Trans = new Vector(transRow);
var transMatrix = new Matrix(Trans.OneDimArray, paramCount, paramCount);
var rmsOfTransRow = rmsOfTransTable.BufferedValues[i];
Vector RmsOfTrans = new Vector(rmsOfTransRow);
var rmsOfTransMatrix = new Matrix(RmsOfTrans.OneDimArray, paramCount, paramCount);
matrixBuilder.SetTransfer(new WeightedMatrix(transMatrix, rmsOfTransMatrix.Pow(2.0)));
}
Process(matrixBuilder);
PerformProcessStep();
}
this.FullProcess();
}
/// <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="ObsMinusApprox"></param>
/// <returns></returns>
public ManualAdjustMatrixBuilder SetObsMinusApprox(WeightedVector ObsMinusApprox)
{
this._ObsMinusApprox = ObsMinusApprox; return(this);
}
/// <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>
/// <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);
}
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="totalFloat"></param>
/// <param name="fixedAmbiguities"></param>
/// <returns></returns>
public WeightedVector GetResultByWeighedParamAdjust(WeightedVector totalFloat, WeightedVector fixedAmbiguities)
{
if (fixedAmbiguities.Count == 0)
{
return(totalFloat);
}
int totalParamCount = totalFloat.Count; //待估参数个数
int fixedAmbiCount = fixedAmbiguities.Count;
//平差系数阵
var PA = new Matrix(totalFloat.Weights);
var LA = new Matrix((IVector)totalFloat, true)
{
RowNames = totalFloat.ParamNames
};
var A = DiagonalMatrix.GetIdentity(totalParamCount);//A x = l
var LB = new Matrix((IVector)fixedAmbiguities, true)
{
RowNames = fixedAmbiguities.ParamNames
};
var PB = new Matrix(fixedAmbiguities.Weights);
Matrix B = BuildCoeefOfFixedToFloat(totalFloat, fixedAmbiguities);
var BTBP = B.Trans * PB;
//A 为单位阵，不需要计算
var NA = PA; //AT * P * A
var UA = PA * LA; //AT * Ｐ * Ｌ
UA.ColNames = new List <string>()
{
"Names"
};
NA.ColNames = totalFloat.ParamNames;
UA.RowNames = totalFloat.ParamNames;
NA.RowNames = totalFloat.ParamNames;
var NB = BTBP * B;
var UB = BTBP * LB;
UB.ColNames = new List <string>()
{
"Names"
};
NB.ColNames = totalFloat.ParamNames;
UB.RowNames = totalFloat.ParamNames;
NB.RowNames = totalFloat.ParamNames;
MatrixEquation neA = new MatrixEquation(NA, UA);
MatrixEquation neB = new MatrixEquation(NB, UB);
var eq = neA + neB;
var result = eq.GetEstimated();
//整体矩阵验证, 2018.10.20, czs, in hmx, 已经验证与整体平差一致，但是如果权太大如1e40，则可能出现舍入误差，而失真！！
if (false)
{
Matrix AB = new Matrix(A.RowCount + B.RowCount, A.ColCount);
AB.SetSub(A);
AB.SetSub(B, A.ColCount);
Matrix PAB = new Matrix(PA.RowCount + PB.RowCount);
PAB.SetSub(PA);
PAB.SetSub(PB, PA.RowCount, PA.ColCount);
Vector LAB = new Vector(totalFloat);
LAB.AddRange(fixedAmbiguities);
ParamAdjuster paramAdjuster = new ParamAdjuster();
var result2 = paramAdjuster.Run(new AdjustObsMatrix(new WeightedVector(LAB, PAB.Inversion), AB));
}
return(result);
}
/// <summary>
/// 设置观测值
/// </summary>
/// <param name="observation"></param>
/// <returns></returns>
public AdjustObsMatrix SetObservation(WeightedVector observation)
{
this.Observation = observation; return(this);
}
public RmsedVector(Adjust.WeightedVector weightedVector)
: base(weightedVector.OneDimArray, weightedVector.ParamNames.ToArray())
{
this.rmsVecror = new List <double>(weightedVector.GetRmsVector().OneDimArray);
}
/// <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>
/// <param name="observation">观测值信息</param>
/// <param name="control">控制矩阵，有时为非对称阵，如PPP</param>
/// <param name="covaOfObs">观测值协方差</param>
private WeightedVector CorrectSimple(IMatrix observation, IMatrix control, IMatrix covaOfObs)
{
//简化字母表示
//先验信息
Matrix Q1 = new Matrix(CovaOfPredictParam);
Matrix X1 = new Matrix(PredictParam);
//观测信息
Matrix L = new Matrix(observation);
Matrix B = new Matrix(control);
Matrix Qo = new Matrix(covaOfObs);
Matrix P1 = new Matrix(CovaOfPredictParam.GetInverse());
Matrix Po = new Matrix(covaOfObs.GetInverse());
//通过Cholesky分解，单位化
CholeskyDecomposition Cholesky1 = new CholeskyDecomposition(P1);
Matrix R0 = new Matrix((Cholesky1.LeftTriangularFactor.Transpose()));//
Matrix z0 = R0 * X1;
CholeskyDecomposition Cholesky2 = new CholeskyDecomposition(Po);
Matrix R = new Matrix((Cholesky2.LeftTriangularFactor.Transpose()));//
Matrix z = R * L;
Matrix A = R * B;
if (R0.ColCount != A.ColCount)
{
//
throw new Exception("What is wrong in SquareRootInformationFilter!");
}
//合并矩阵
Matrix ConbRA = ConbineMatrixByCol(R0, A);
Matrix ConbZ0Z = ConbineMatrixByCol(z0, z);
//正交化
HouseholderTransform HouseholderTransform = new HouseholderTransform(ConbRA);
Matrix T = HouseholderTransform.T;
//更新
Matrix newConbRA = T * ConbRA;
Matrix newConbZ0Z = T * ConbZ0Z;
//
//提取
Matrix newR0 = newConbRA.GetSub(0, 0, R0.RowCount, R0.ColCount);
Matrix newZ0 = newConbZ0Z.GetSub(0, 0, z0.RowCount, z0.ColCount);
//解算
Matrix newR0_Cov = new Matrix(newR0.GetInverse());
Matrix X = newR0_Cov * newZ0;
Matrix QX = newR0_Cov * newR0_Cov.Transpose();
BuildCovaFactor(L, B, Po, P1, X1, X);
if (false)
{
var III = newR0_Cov * newR0_Cov;
int i = 0;
i = 0;
}
var Estimated = new WeightedVector(X, QX)
{
ParamNames = ObsMatrix.ParamNames
};
return(Estimated);
}
/// <summary>
/// 参数先验值
/// </summary>
/// <param name="AprioriParam"></param>
/// <returns></returns>
public ManualAdjustMatrixBuilder SetAprioriParam(WeightedVector AprioriParam)
{
this._AprioriParam = AprioriParam; return(this);
}
/// <summary>
/// 估计，改正。通常采用的方法。From 崔阳、2017.06.22
/// </summary>
/// <param name="observation"></param>
/// <param name="control"></param>
/// <param name="covaOfObs"></param>
/// <returns></returns>
private WeightedVector NewCorrect(IMatrix observation, IMatrix control, IMatrix covaOfObs)
{
Matrix Q1 = new Matrix(CovaOfPredictParam);
Matrix P1 = new Matrix(CovaOfPredictParam.GetInverse());
Matrix X1 = new Matrix(PredictParam);
Matrix L = new Matrix(observation);
Matrix A = new Matrix(control);
Matrix AT = new Matrix(A.Transposition);
Matrix Po = new Matrix(covaOfObs.GetInverse());
Matrix Qo = new Matrix(covaOfObs);
//计算新息向量
Matrix Vk1 = A * X1 - L;
Matrix PXk = null;
if (Q1.IsDiagonal)
{
var atpa = ATPA(AT, Q1);
//IMatrix at = AT.Multiply(P_o).Multiply(control);
PXk = new Matrix(atpa + Qo);
}
else
{
PXk = A * Q1 * AT + Qo;//平差值Xk的权阵
}
//计算平差值的权逆阵
Matrix CovaOfP = PXk.Inversion;//.GetInverse();
//计算增益矩阵
//IMatrix J = Q1.Multiply(AT).Multiply(CovaOfP);
Matrix J = Q1 * AT * CovaOfP;
//计算平差值
//IMatrix X = PredictParam.Minus(J.Multiply(Vk1));
Matrix X = X1 - J * Vk1;
Matrix I = Matrix.CreateIdentity(Q1.ColCount);
#region 理论公式
Matrix t2 = J * A; // (J.Multiply(A));
Matrix t3 = I - t2; // I.Minus(t2);
Matrix Qx = t3 * Q1; // (t3).Multiply(Q1);
#endregion
#region 阮论文公式
//IMatrix t21 = I.Minus(J.Multiply(control));
//IMatrix t22 = I.Minus(controlT.Multiply(J.Transposition));
//IMatrix t3 = J.Multiply(covaOfObs.Multiply(J.Transposition));
//IMatrix CovaOfEstParam = ((t21).Multiply(CovaOfPredictParam).Multiply(t22)).Plus(t3);
#endregion
var Estimated = new WeightedVector(X, Qx)
{
ParamNames = ObsMatrix.ParamNames
};
BuildCovaFactor(L, A, Po, P1, X1, X);
return(Estimated);
}
/// <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);
}
