/// <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="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); }