/// <summary> /// 初始化。 /// </summary> /// <param name="coeffOfParams">参数(误差方程)系数阵</param> /// <param name="obsMinusApriori">误差方程右手边</param> /// <param name="inverseWeight">观测方程权逆阵</param> /// <param name="coeff_condition">条件方程系数阵</param> /// <param name="obsMinusApriori_condition">条件方程常数项</param> public void Init(double[][] coeffOfParams, double[][] obsMinusApriori, double[][] inverseWeight, double[][] coeff_condition, double[][] obsMinusApriori_condition) { ArrayMatrix Q, P; if (inverseWeight == null)//若为空,则为单位阵 { Q = new ArrayMatrix(MatrixUtil.CreateIdentity(coeffOfParams.Length)); P = Q; } else { Q = new ArrayMatrix(inverseWeight); P = Q.Inverse; } this.Coeff_condition = coeff_condition; this.ObsMinusApriori_condition = obsMinusApriori_condition; this.A = new ArrayMatrix(coeffOfParams); this.L = new ArrayMatrix(obsMinusApriori); ArrayMatrix AT = A.Transpose(); this.Weight = P.Array; this.ObsCount = coeffOfParams.Length; this.Normal_error = (AT * P * A).Array; this.RightHandSide_error = (AT * P * L).Array; this.Normal_condition = (new ArrayMatrix(coeff_condition) * new ArrayMatrix(Normal_error) * new ArrayMatrix(coeff_condition).Transpose()).Array; }
/// 初始化并计算。 /// </summary> /// <param name="coeff_param">参数的系数阵</param> /// <param name="obsMinusApriori">观测值减去估值</param> /// <param name="inverseWeight_obs">观测值的权逆阵</param> /// <param name="inverseWeight_signal">含下面最后三个变量</param> public void Init( double[][] coeff_param, //参数的系数阵 double[][] obsMinusApriori, //观测值减去估值 double[][] inverseWeight_obs, //观测值的权逆阵 double[][] inverseWeight_signal // 含下面最后三个变量 ) { int paramCount = coeff_param[0].Length; int allSignalCount = inverseWeight_signal[0].Length; int measuredSignalCount = paramCount; int notMeasuredSignalCount = allSignalCount - measuredSignalCount; //生成信号的系数阵 double[][] coeff_signal = MatrixUtil.Create(paramCount, allSignalCount);//是否应该自己生成 MatrixUtil.SetSubMatrix(coeff_signal, MatrixUtil.CreateIdentity(paramCount)); //提取方差、协方差阵 double[][] inverseWeight_mesuredSignal = MatrixUtil.GetSubMatrix(inverseWeight_signal, measuredSignalCount, measuredSignalCount); //inverseWeight_signal的左上角 double[][] inverseWeight_notMesuredSignal = MatrixUtil.GetSubMatrix(inverseWeight_signal, notMeasuredSignalCount, notMeasuredSignalCount, measuredSignalCount, measuredSignalCount); //inverseWeight_signal的右下角 double[][] covaSignals_Ss = MatrixUtil.GetSubMatrix(inverseWeight_signal, 0, notMeasuredSignalCount, measuredSignalCount, measuredSignalCount); //inverseWeight_signal的右上角 Init( coeff_param, //参数的系数阵 obsMinusApriori, //观测值减去估值 inverseWeight_obs, //观测值的权逆阵 inverseWeight_signal, // 含下面最后三个变量 coeff_signal, //是否应该自己生成 inverseWeight_mesuredSignal, //inverseWeight_signal的左上角 inverseWeight_notMesuredSignal, //inverseWeight_signal的右下角 covaSignals_Ss //inverseWeight_signal的右上角 ); }
/// <summary> /// 在 GNSS 动态导航中的应用 /// </summary> protected void Motion() { List <string> paramNames = new List <string>() { "X", "Y", "Z", "dX", "dY", "dZ" }; ///参数 List <Vector> satXyzs = new List <Vector>(); Vector siteXyz = new Vector(3); Vector L = new Vector(6); //观测值 double interval = 30; //时间间隔(单位:秒) //将瞬时加速度作为随机干扰, //状态向量, 位置和速度 X = trans([x, y, z, vx, vy, vz]) //噪声向量, 为加速度 Noise = trans(ax, ay, az]) //纯量形式, y = y0 + t * v0 + 0.5 * a0 * a0 * t // v = v0 + a0 * t /// 状态方程 /// ┌I Δt┐ ┌0.5*Δt*Δt┐ /// X(k) = │ │X(k-1) + │ │Ω(k-1) /// └0 I ┘ └ Δt ┘ /// 观测方程 /// L(k) = A(k) X(k) + Ω(k) /// 其中,L(k)为 k 时刻系统的 C/A 码观测向量 //常用量 double[][] identity3x3 = MatrixUtil.CreateIdentity(3); int satCount = satXyzs.Count; //状态方程状态转移矩阵 Φ(k,k-1) // double[][] trans = MatrixUtil.CreateIdentity(6); ArrayMatrix trans = new ArrayMatrix(6, 6); MatrixUtil.SetSubMatrix(trans.Array, MatrixUtil.GetMultiply(identity3x3, interval), 0, 3); //状态方程噪声向量Ω(k-1)的系数阵 double[][] coeef_noise = MatrixUtil.Create(6, 3); MatrixUtil.SetSubMatrix(coeef_noise, MatrixUtil.GetMultiply(identity3x3, 0.5 * interval * interval)); MatrixUtil.SetSubMatrix(coeef_noise, MatrixUtil.GetMultiply(identity3x3, interval), 3); //观测方程系数矩阵 A,每个时刻不同 ArrayMatrix coeef_obs = new ArrayMatrix(satCount, 6); for (int i = 0; i < satCount; i++) { IVector differXyz = (satXyzs[i] - siteXyz); coeef_obs[i, 0] = differXyz.GetCos(0);//.CosX; coeef_obs[i, 1] = differXyz.GetCos(1); coeef_obs[i, 2] = differXyz.GetCos(2); } KalmanFilter lastKf = null; //Vector lastParams = lastKf.Corrected ?? null; //IMatrix lastParamCova = lastKf.Corrected.InverseWeight ?? null; //KalmanFilter kf = new KalmanFilter(coeef_obs, L, trans, lastParams, lastParamCova); //kf.Process(); }
/// <summary> /// 测试分区平差 /// </summary> public static void Test(int allObsCount = 1000, bool useParalell = true, int core = 8, int blockCount = 8) { DateTime from = DateTime.Now; //观测噪声 Random random = new Random(); //设有blockObsCount个观测量,paramCount个区内参数,commonParamCount个公共参数 int blockObsCount = allObsCount / blockCount; int paramCount = blockObsCount / 2 + 1; int commonParamCount = blockObsCount / 4 + 1; List <BlockAdjustItem> items = new List <BlockAdjustItem>(); for (int m = 0; m < blockCount; m++) { //分区内参数系数阵 obsCount x paramCount double[][] coeffA = GenCoeffA(blockObsCount, paramCount); //观测值 obsCount x 1 double[][] obs = MatrixUtil.Create(blockObsCount, 1); for (int i = 0; i < blockObsCount; i++) { obs[i][0] = i; // +random.NextDouble() - 0.5; } //观测值权逆阵 obsCount x obsCount double[][] inverseOfObs = MatrixUtil.CreateIdentity(blockObsCount); //分区内对公共参数的系数阵 obsCount x commonParamCount double[][] coeffB = MatrixUtil.Create(blockObsCount, commonParamCount); for (int i = 0; i < commonParamCount; i++) { coeffB[i][i] = 1; } //System.IO.File.WriteAllText( // "C:\\Block"+m+".txt", // "A" + "\r\n" + MatrixUtil.GetFormatedText(coeffA) + "\r\n" // + "L" + "\r\n" + MatrixUtil.GetFormatedText(obs) + "\r\n" // + "Q" + "\r\n" + MatrixUtil.GetFormatedText(inverseOfObs) + "\r\n" // + "Ab" + "\r\n" + MatrixUtil.GetFormatedText(coeffB) + "\r\n" // ); BlockAdjustItem item = new BlockAdjustItem(coeffA, obs, inverseOfObs, coeffB); items.Add(item); } BlockAdjustment ba = new BlockAdjustment(items.ToArray(), useParalell, core); TimeSpan span = DateTime.Now - from; StringBuilder sb = new StringBuilder(); sb.AppendLine("耗时:" + span + " = " + span.TotalMinutes + " 分"); System.IO.File.WriteAllText("C:\\IsP" + useParalell.ToString() + "obs" + +allObsCount + "_core" + core + "_block" + blockCount + ".txt", sb.ToString() + ba.ToString()); }
private double[][] CreateTrans(double deltaT) { double[][] trans = MatrixUtil.CreateIdentity(ParamCount); //右上角的对角阵 for (int j = 0; j < 4; j++) { trans[j][j + 4] = deltaT; } return(trans); }
/// <summary> /// 先验参数 /// </summary> protected override WeightedVector CreateInitAprioriParam() { double[] residualVec = new double[ParamCount]; //近似差为 0 double[][] inverseWeight = MatrixUtil.CreateIdentity(ParamCount); inverseWeight[3][3] = 100000.0; //钟差 inverseWeight[7][7] = 5e-12; //kyc:钟漂,谱哥说给个小的值 if (Option.IsIndicatingApproxXyzRms) { int i = 0; for (i = 0; i < 3; i++) { var item = Option.InitApproxXyzRms[i]; inverseWeight[i][i] = item * item; } for (int j = 4; j < 7; j++)//kyc { inverseWeight[j][j] = 10000; } } else if (CurrentMaterial.SiteInfo.EstimatedXyz.IsZero) { for (int i = 0; i < 3; i++) { inverseWeight[i][i] = 1e10; } for (int j = 4; j < 7; j++) { inverseWeight[j][j] = 1e10; } //kyc } else { for (int i = 0; i < 3; i++) { inverseWeight[i][i] = 1000; } for (int j = 4; j < 7; j++) { inverseWeight[j][j] = 1000; } //kyc } return(new WeightedVector(residualVec, inverseWeight)); }
/// <summary> /// 测试参数加权平差 /// </summary> public static void Test() { Random random = new Random();//观测噪声 double[][] A = MatrixUtil.CreateIdentity(6); double[][] L = MatrixUtil.Create(6, 1); for (int i = 0; i < L.Length; i++) { L[i][0] = i + 1 + random.NextDouble() - 0.5; } double[][] QL = MatrixUtil.CreateDiagonal(6, 1); double[][] Xa0 = MatrixUtil.Create(3, 1); for (int i = 0; i < Xa0.Length; i++) { Xa0[i][0] = i + 1; } double[][] Qx0 = MatrixUtil.CreateDiagonal(3, 0.001); // WeightedParamAdjustment adjust = new WeightedParamAdjustment(A, L, QL, Xa0, Qx0); }
/// <summary> /// 先验参数 /// </summary> protected override WeightedVector CreateInitAprioriParam() { double[] residualVec = new double[ParamCount];//近似差为 0 double[][] inverseWeight = MatrixUtil.CreateIdentity(ParamCount); if (Option.IsIndicatingApproxXyzRms) { int i = 0; for (i = 0; i < 3; i++) { var item = Option.InitApproxXyzRms[i]; inverseWeight[i][i] = item * item; } } else if (CurrentMaterial.SiteInfo.EstimatedXyz.IsZero) { for (int i = 0; i < 3; i++) { inverseWeight[i][i] = 1e10; } } else { for (int i = 0; i < 3; i++) { inverseWeight[i][i] = 100000; } } inverseWeight[3][3] = 1000000;//钟差 for (int i = 4; i < 3 + Option.SatelliteTypes.Count; i++) { inverseWeight[i][i] = 0.25;//系统时间偏差 } return(new WeightedVector(residualVec, inverseWeight)); }
/// <summary> /// Ambizap算法的实现。 /// 选择两个点的PPP计算结果,及其基线计算结果。 /// </summary> private void Read() { string pointNameA = null, pointNameB = null; //---------------------------------------------------数据读取//--------------------------------------------------- //只提取两个点的位置信息及其基线。 //读取PPP结果 string[] pppfilePathes = this.textBox_PathesOfPPP.Lines; List <SinexFile> pppsinexFiles = SinexReader.Read(pppfilePathes); List <SinexSiteDetail> pppSitesA = new List <SinexSiteDetail>(); List <SinexSiteDetail> pppSitesB = new List <SinexSiteDetail>(); foreach (var file in pppsinexFiles) { List <SinexSiteDetail> sites = file.GetSinexSites(); foreach (var item2 in sites) { if (pointNameA == null) { pointNameA = item2.Name; } if (pointNameB == null && pointNameA != item2.Name) { pointNameB = item2.Name; } if (item2.Name == pointNameA) { pppSitesA.Add(item2); } if (item2.Name == pointNameB) { pppSitesB.Add(item2); } } } if (pointNameA == null || pointNameB == null) { throw new ArgumentException("起算数据不足。"); } //读取基线结果 //只需要一条基线就行了。 string[] baseLineFilePathes = this.textBox_pathesOfBaseLine.Lines; List <SinexFile> baseLineSinexFiles = SinexReader.Read(baseLineFilePathes); List <SinexSiteDetail> baseLineSites = new List <SinexSiteDetail>(); foreach (var file in baseLineSinexFiles) { List <SinexSiteDetail> sites = file.GetSinexSites(); foreach (var item2 in sites) { if ((item2.Name == pointNameA || item2.Name == pointNameB) && (baseLineSites.Find(m => m.Name == item2.Name) == null)) { baseLineSites.Add(item2); } if (baseLineSites.Count == 2) { break; } } if (baseLineSites.Count == 2) { break; } } //组建基线 string name = baseLineSites[0].Name + "-" + baseLineSites[1].Name; BaseLine line = new BaseLine(name, baseLineSites[1].EstimateXYZ - baseLineSites[0].EstimateXYZ); //---------------------------------------------------------平差计算-------------------------------------------- //以第一个坐标为近似值 XYZ aprioriA = pppSitesA[0].EstimateXYZ; XYZ aprioriB = pppSitesB[0].EstimateXYZ; //构建矩阵,具有约束条件的参数平差中,误差方程和约束方程的未知数相同。 //参数有6个x0,y0,z0,x1,y1,z1,条件有三个 x1-x0 = a, y1-y0 = b, z1-z0 = c double[][] coeff_error = MatrixUtil.Create((pppSitesA.Count + pppSitesB.Count) * 3, 6); double[][] indentity3x3 = MatrixUtil.CreateIdentity(3); for (int i = 0; i < pppSitesA.Count; i++) { MatrixUtil.SetSubMatrix(coeff_error, indentity3x3, i * 3, 0); } for (int i = 0; i < pppSitesB.Count; i++) { MatrixUtil.SetSubMatrix(coeff_error, indentity3x3, (pppSitesA.Count + i) * 3, 3); } //obsMinusApriori double[] obsMinusApriori_error = new double[(pppSitesA.Count + pppSitesB.Count) * 3]; for (int i = 0; i < pppSitesA.Count; i++) { int rowIndex = i * 3; obsMinusApriori_error[rowIndex + 0] = pppSitesA[i].EstimateXYZ.X - aprioriA.X; obsMinusApriori_error[rowIndex + 1] = pppSitesA[i].EstimateXYZ.Y - aprioriA.Y; obsMinusApriori_error[rowIndex + 2] = pppSitesA[i].EstimateXYZ.Z - aprioriA.Z; } for (int i = 0; i < pppSitesB.Count; i++) { int rowIndex = (pppSitesA.Count + i) * 3; obsMinusApriori_error[rowIndex + 0] = pppSitesB[i].EstimateXYZ.X - aprioriB.X; obsMinusApriori_error[rowIndex + 1] = pppSitesB[i].EstimateXYZ.Y - aprioriB.Y; obsMinusApriori_error[rowIndex + 2] = pppSitesB[i].EstimateXYZ.Z - aprioriB.Z; } //Coeff_Condition,条件有三个 x0-x1 - a = 0, //条件方程:c x + w = 0. => c deltaX + c X0 + w = 0 double[][] coeff_condition = MatrixUtil.Create(3, 6); for (int index = 0; index < 3; index++) { coeff_condition[index][index + 0] = -1; coeff_condition[index][index + 3] = 1; } //ConstVector_Condition c deltaX - W = 0, W = w - c X0 double[] obsMinusApriori_condition = new double[3]; XYZ aprioriVector = aprioriB - aprioriA; obsMinusApriori_condition[0] = line.Vector.X - aprioriVector.X; obsMinusApriori_condition[1] = line.Vector.Y - aprioriVector.Y; obsMinusApriori_condition[2] = line.Vector.Z - aprioriVector.Z; ParamAdjustmentWithCondition adjust = new ParamAdjustmentWithCondition(coeff_error, obsMinusApriori_error, coeff_condition, obsMinusApriori_condition); double[][] param = adjust.Solve(); //---------------------------------------------------------结果输出-------------------------------------------- StringBuilder sb = new StringBuilder(); sb.AppendLine("计算完毕"); XYZ estA = aprioriA + new XYZ(adjust.ParamVector); XYZ estB = aprioriB + new XYZ(adjust.ParamVector[3], adjust.ParamVector[4], adjust.ParamVector[5]); sb.AppendLine("近似值A:" + aprioriA); sb.AppendLine("平差值A:" + estA); sb.AppendLine("近似值B:" + aprioriB); sb.AppendLine("平差值B:" + estB); sb.AppendLine("条件向量 A->B:" + line.Vector); sb.AppendLine("平差向量 A->B:" + (estB - estA)); sb.AppendLine("------------------------------------参数------------------------------------"); sb.AppendLine(MatrixUtil.GetFormatedText(param)); double[][] param2 = adjust.Solve_ToBeCheck(); sb.AppendLine("param2"); sb.AppendLine(MatrixUtil.GetFormatedText(param2)); sb.AppendLine("误差方程系数阵"); sb.AppendLine(MatrixUtil.GetFormatedText(coeff_error)); sb.AppendLine("------------------------------------obsMinusApriori------------------------------------"); sb.AppendLine(MatrixUtil.GetFormatedText(MatrixUtil.Create(obsMinusApriori_error))); sb.AppendLine("------------------------------------Coeff_Condition------------------------------------"); sb.AppendLine(MatrixUtil.GetFormatedText(coeff_condition)); sb.AppendLine("------------------------------------ConstVector_Condition------------------------------------"); sb.AppendLine(MatrixUtil.GetFormatedText(MatrixUtil.Create(obsMinusApriori_condition))); sb.AppendLine("------------------------------------误差方程法方程------------------------------------"); sb.AppendLine(MatrixUtil.GetFormatedText(adjust.Normal_error)); sb.AppendLine("------------------------------------条件方程法方程------------------------------------"); sb.AppendLine(MatrixUtil.GetFormatedText(adjust.Normal_condition)); sb.AppendLine("------------------------------------误差方程右手边------------------------------------"); sb.AppendLine(MatrixUtil.GetFormatedText(adjust.RightHandSide_error)); this.textBox_info.Text = sb.ToString(); //----------------------------------------------------地图准备//---------------------------------------------------- points.Clear(); GeoCoord c1 = CoordTransformer.XyzToGeoCoord(aprioriA); GeoCoord c2 = CoordTransformer.XyzToGeoCoord(aprioriB); GeoCoord c3 = CoordTransformer.XyzToGeoCoord(estA); GeoCoord c4 = CoordTransformer.XyzToGeoCoord(estB); points.Add(new AnyInfo.Geometries.Point(c1.Lon, c1.Lat, "aprA")); points.Add(new AnyInfo.Geometries.Point(c2.Lon, c2.Lat, "aprB")); points.Add(new AnyInfo.Geometries.Point(c3.Lon, c3.Lat, "estA")); points.Add(new AnyInfo.Geometries.Point(c4.Lon, c4.Lat, "estB")); }