/// <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 * P * L 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); }