private AdjustResultMatrix GetRealTimeResult(AdjustObsMatrix input) { //构建不变参数的法方程 var newConstParamNe = input.BuildConstParamNormalEquation(); NormalEquationSuperposer.Add(newConstParamNe); //添加到法方程迭加器中 WeightedVector estY = NormalEquationSuperposer.GetEstimated(); AdjustResultMatrix result = Step3GetMutableX(estY, ObsMatrix); //求异变参数 return(result); }
private static WeightedVector Step2GetConstY(List <MatrixEquation> normals) { NormalEquationSuperposer NormalEquationSuperPoser = new NormalEquationSuperposer(); //step 2: 求固定参数 foreach (var normal in normals) { NormalEquationSuperPoser.Add(normal); } return(NormalEquationSuperPoser.GetEstimated()); }
/// <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); }