/// <summary> /// 快捷计算方法,返回 SymmetricMatrix /// </summary> /// <param name="A">系数阵</param> /// <param name="P">对角阵</param> /// <returns></returns> public static Matrix ATPA(IMatrix A, IMatrix P) { return(new Matrix(AdjustmentUtil.ATPA(A, P))); }
/// <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 = new Matrix(AdjustmentUtil.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> 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> 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); }