/// <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="input"></param> /// <returns></returns> public override AdjustResultMatrix Run(AdjustObsMatrix input) { //try //{ this.ObsMatrix = input; int paramCount = input.Coefficient.ColCount; Predict(input.Transfer, InverseWeightOfTransfer); var est1 = CorrectSimple(input.Observation - input.FreeVector, input.Coefficient, input.Observation.InverseWeight); // var est2 = CorrectNormal(this.Observation, CoeffOfParam, Observation.InverseWeight); // var est3 = NewCorrect(this.Observation, CoeffOfParam, Observation.InverseWeight); //var differ12 = est1 - est2; //var differ23 = est2 - est3; //var differ13 = est1 - est3; var Estimated = est1; AdjustResultMatrix result = new AdjustResultMatrix() .SetEstimated(Estimated) .SetObsMatrix(input) .SetFreedom(Freedom) .SetVarianceFactor(VarianceOfUnitWeight) .SetVtpv(vtpv) ; return(result); //} //catch (Exception ex) //{ // log.Error("SimpleKalmanFilter 滤波出错了", ex); //} }
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); }
/// <summary> /// 实时计算,数据计算 /// </summary> /// <param name="input">观测矩阵</param> /// <returns></returns> public override AdjustResultMatrix Run(AdjustObsMatrix input) { if (input == this.ObsMatrix) { return(this.LastResult); } this.ObsMatrix = input; switch (StepOfRecursive) { case StepOfRecursive.SuperposOfConstNeq: return(GetConstParamResult(input, NormalEquationSuperposer)); //case StepOfRecursive.ComputeMutableParam: break; case StepOfRecursive.SequentialConst: AdjustResultMatrix res = GetSequentialConst(input); this.LastConstResult = res; return(res); case StepOfRecursive.RealTime: //实时计算,参数变化可能带来错误 AdjustResultMatrix result = GetRealTimeResult(input); this.LastResult = result; return(result); case StepOfRecursive.ParamAdjust: //参数逐历元平差 return(GetSimpleParamAdjustResult(input)); //参数平差验证,2018.10.15, czs, hmx, 验证第一个结果是一样的。 default: break; } return(null); }
public override AdjustResultMatrix Run(AdjustObsMatrix input) { var result = ConditionalAdjuster.Run(input); throw new NotImplementedException(); }
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> /// <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> 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> private static AdjustResultMatrix GetSimpleParamAdjustResult(AdjustObsMatrix input) { #region 合并系数阵 Matrix A = new Matrix(input.Coefficient.RowCount, input.Coefficient.ColCount + input.SecondCoefficient.ColCount); A.SetSub(input.Coefficient); A.SetSub(input.SecondCoefficient, 0, input.Coefficient.ColCount); var names = new List <string>(); names.AddRange(input.ParamNames); names.AddRange(input.SecondParamNames); #endregion ParamAdjuster paramAdjuster = new ParamAdjuster(); AdjustObsMatrix adjustObs = new AdjustObsMatrix(input.Observation, A, null, names); var resultMatrix = paramAdjuster.Run(adjustObs); return(resultMatrix); }
/// <summary> /// 解析 /// </summary> /// <param name="text"></param> /// <param name="splitterOfMatrixItems">分隔矩阵元素的分隔符</param> /// <returns></returns> public static AdjustObsMatrix Parse(string text, string[] splitterOfMatrixItems = null) { AdjustObsMatrix obs = new AdjustObsMatrix(); if (splitterOfMatrixItems == null) { splitterOfMatrixItems = new string[] { ",", ";", "\t", " " }; } string[] lines = text.Split(new char[] { '\r', '\n' }, StringSplitOptions.None); //保留空格以分隔 //解析本对象属性 ParseHeader(obs, lines); int len = lines.Length; var typeNames = Enum.GetNames(typeof(ObsMatrixType)); StringBuilder currentBlockSb = new StringBuilder(); ObsMatrixType currentType = ObsMatrixType.Apriori; ObsMatrixType nextType = ObsMatrixType.Apriori; for (int i = 0; i < len; i++) { string line = lines[i].Trim(); //当前行 if (typeNames.Contains(line)) //新区 { currentType = nextType; // nextType = Geo.Utils.EnumUtil.Parse <ObsMatrixType>(line); //这是解析的下一个区的名称 if (currentBlockSb.Length > 2) { ParseBufferText(splitterOfMatrixItems, obs, currentBlockSb, currentType); } continue; } currentBlockSb.AppendLine(line); } if (currentBlockSb.Length > 2) { currentType = nextType;// //解析最后一个 ParseBufferText(splitterOfMatrixItems, obs, currentBlockSb, currentType); } return(obs); }
/// <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> /// <param name="obs"></param> /// <param name="lines"></param> private static void ParseHeader(AdjustObsMatrix obs, string[] lines) { var headerSpliter = new char[] { ':', ':' }; var headerItemSpliter = new char[] { ',', ' ', '\t', ';' }; foreach (var line in lines) { if (String.IsNullOrEmpty(line)) { continue; } if (line.Contains("×")) { break; } var items = line.Split(headerSpliter, StringSplitOptions.RemoveEmptyEntries); if (items.Length == 1) { continue; } var header = items[0]; var content = items[1]; switch (header) { case "ParamNames": obs.ParamNames = new List <string>(content.Split(headerItemSpliter, StringSplitOptions.RemoveEmptyEntries)); break; case "SecondParamNames": obs.SecondParamNames = new List <string>(content.Split(headerItemSpliter, StringSplitOptions.RemoveEmptyEntries)); break; default: break; } } }
/// <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="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); }
/// <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> 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> /// <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); }
/// <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="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 B = new Matrix(input.SecondCoefficient); Matrix BT = B.Trans; Matrix X0 = input.HasApprox ? new Matrix(input.ApproxVector, true) : null; Matrix Y0 = input.HasSecondApprox ? new Matrix(input.SecondApproxVector, true) : null; Matrix D = input.HasFreeVector ? new Matrix(input.FreeVector, true) : null; int obsCount = L.RowCount; int fixedParamCount = B.ColCount; //观测值更新 Matrix l = L - (A * X0 + B * Y0 + D); //如果null,则是本身 Matrix ATPL = AT * PL; //法方程 Matrix Na = ATPL * A; Matrix Nab = AT * PL * B; Matrix InverNa = Na.Inversion; Matrix J = A * InverNa * AT * PL; Matrix B2 = (Matrix.CreateIdentity(J.RowCount) - J) * B; Matrix AcT = B2.Trans; Matrix Nc = AcT * PL * B2;; Matrix InverNc = Nc.Inversion; //平差结果 Matrix y = InverNc * AcT * PL * l; Matrix Y = Y0 + y; Matrix Qy = InverNc; //只针对y的精度评定 Matrix W = B2 * y - l; double ys0 = (W.Trans * PL * W).FirstValue / (obsCount - fixedParamCount); Matrix Dy = ys0 * Qy; //求x Matrix lx = L - (A * X0 + B * Y + D); //如果null,则是本身 Matrix x = InverNa * ATPL * lx; //这两个计算是等价的 // Matrix x = InverNa * (ATPL * l - Nab * y);//这两个计算是等价的 Matrix X = X0 + x; Matrix Ntmp = Na.Inversion * Nab; Matrix Qx = InverNa + Ntmp * Qy * Ntmp.Trans; //Matrix Qxy = AT * PL * B; //Matrix Qtemp = Qx * Qxy; //Matrix QtempT = Qtemp.Trans; //Matrix Dx = Qx + Qtemp * Dy * QtempT; //精度评定 Matrix V = A * x + B * y - l; Matrix Qv = QL - A * Qx * AT - B * Qy * BT; // Matrix lT = l.Trans; double vtpv = (V.Trans * PL * V)[0, 0];//(lT * PL * l - lT * PL * Ac * y).FirstValue;// int paramCount = A.ColCount + B.ColCount; int freedom = A.RowCount - paramCount; 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 estY = new WeightedVector(y, Qy) { ParamNames = input.SecondParamNames }; WeightedVector estV = new WeightedVector(V, Qv); 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(input) .SetFreedom(freedom) .SetVarianceFactor(s0) .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); }
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); }