// 根据强信号点推理干扰源,多点定位 private void strongLoc() { List <List <int> > id = getStrongPtMult(); // 获得多点 MulPtLoc solve = new MulPtLoc((leftBound + rightBound) / 2, (downBound + upBound) / 2, 6); for (int i = 0; i < id.Count; i++) { List <List <double> > X = new List <List <double> >(); // 测量点位置 List <double> P = new List <double>(); // 测量点功率 for (int j = 0; j < id[i].Count; j++) { int k = id[i][j]; X.Add(new List <double> { gx[k], gy[k] }); P.Add(pwrDbm[k]); } double[] loc = solve.solve(ref X, ref P); Pt3D pt = new Pt3D(loc[0], loc[1], 0); if (inBound(ref pt)) { StrongResult.Add(pt); } } }
bool inBound(ref Pt3D pt) { if (pt.x < rightBound && pt.y < upBound && pt.x > leftBound && pt.y > downBound) { return(true); } return(false); }
// 根据强弱相间点对栅格打分 private Result strongWeakLoc() { Pt.Sort(new StrongWeakPtCmp()); // 加载加速结构 RayTracing inter = new RayTracing(); inter.SinglePrepare(leftBound, rightBound, downBound, upBound); // 从每个点发出360*360条射线,经过的格子分值加1,分数最高的格子为可能位置 Dictionary <string, int> dic = new Dictionary <string, int>(); for (int i = 0; i < Pt.Count && i < 20; i++) { for (int j = 0; j < 360; j++) { for (int h = 3; h < 100; h += 3) { double angle = (double)(j) / 180.0 * Math.PI; LTE.Geometric.Point endPoint = new Geometric.Point(Pt[i].strong.X + Math.Cos(angle), Pt[i].strong.Y + Math.Sin(angle), (double)h); LTE.Geometric.Vector3D dir = LTE.Geometric.Vector3D.constructVector(Pt[i].strong, endPoint); inter.SingleRayJudge(Pt[i].strong, dir, ref dic, true); LTE.Geometric.Point endPoint1 = new Geometric.Point(Pt[i].weak.X + Math.Cos(angle), Pt[i].weak.Y + Math.Sin(angle), (double)h); LTE.Geometric.Vector3D dir1 = LTE.Geometric.Vector3D.constructVector(Pt[i].weak, endPoint1); inter.SingleRayJudge(Pt[i].weak, dir1, ref dic, false); } } } if (dic.Count == 0) { return(new Result(false, "强弱点对未推出合理结果")); } // 挑选分数最高的格子 var maxKey = (from d in dic orderby d.Value descending select d.Key).First(); var allMaxKey = (from d in dic where d.Value == dic[maxKey] select d.Key).ToArray(); int cnt = 0; double X = 0, Y = 0, Z = 0; if (allMaxKey.Count() > 5) { foreach (string a in allMaxKey) // 有多个分数最高的格子 { string[] num = a.Split(','); GridHelper.getInstance().AccGridToXYZ(Convert.ToInt32(num[0]), Convert.ToInt32(num[1]), Convert.ToInt32(num[2]), ref X, ref Y, ref Z); Pt3D pt = new Pt3D(X, Y, Z); if (inBound(ref pt)) { StrongWeakResult.Add(pt); } } } else { var dicSort = from objDic in dic orderby objDic.Value descending select objDic; foreach (KeyValuePair <string, int> kvp in dicSort) { string[] num = kvp.Key.Split(','); GridHelper.getInstance().AccGridToXYZ(Convert.ToInt32(num[0]), Convert.ToInt32(num[1]), 0, ref X, ref Y, ref Z); ++cnt; Pt3D pt = new Pt3D(X, Y, Z); if (inBound(ref pt)) { StrongWeakResult.Add(pt); } if (cnt > 5) { break; } } } return(new Result(true)); }
// 根据单调性对栅格打分 private Result monotoneLoc() { int agMaxX = 0, agMaxY = 0, agMinX = 0, agMinY = 0, agZ = 0; GridHelper.getInstance().XYZToAccGrid(leftBound, downBound, 0, ref agMinX, ref agMinY, ref agZ); GridHelper.getInstance().XYZToAccGrid(rightBound, upBound, 0, ref agMaxX, ref agMaxY, ref agZ); Dictionary <string, int> dicMon = new Dictionary <string, int>(); Regress regress1 = new Regress(); for (int div = 0; div < roadDivide.Count - 1; div++) { List <double> pwrDbmSub = new List <double>(); List <double> x = new List <double>(); x.Add(0); for (int j = roadDivide[div]; j < roadDivide[div + 1]; j++) { pwrDbmSub.Add(pwrDbm[j]); x.Add(j - roadDivide[div] + 1); } #region 提取信号强度突变点 Divide divide = new Divide(100, 0.8, ref pwrDbmSub); divide.run(); #endregion #region 对每段进行线性拟合 for (int i = 0; i < divide.posV.Count - 1; i++) { int id1 = divide.posV[i]; int id2 = divide.posV[i + 1] - 1; if (id2 < 0) { id2 = 0; } double a2 = 0, b2 = 0, err2 = 0; regress1.CalcRegress(ref x, divide.m, id1, id2, out a2, out b2, out err2); // y = ax + b double x1, y1, x2, y2; // (x1, y1) 单调上升的尽头 if (a2 < 0) { x1 = gx[id1 + roadDivide[div]]; y1 = gy[id1 + roadDivide[div]]; x2 = gx[id2 + roadDivide[div]]; y2 = gy[id2 + roadDivide[div]]; } else { x1 = gx[id2 + roadDivide[div]]; y1 = gy[id2 + roadDivide[div]]; x2 = gx[id1 + roadDivide[div]]; y2 = gy[id1 + roadDivide[div]]; } double k = (x1 - x2) / (y2 - y1); double b = y1 - (x1 - x2) / (y2 - y1) * x1; double y2Tmp = k * x2 + b; if (y2 < y2Tmp) { for (int tx = agMinX + 1; tx < agMaxX - 1; tx++) { for (int ty = agMinY + 1; ty < agMaxY - 1; ty++) { double cx = 0, cy = 0, cz = 0; GridHelper.getInstance().AccGridToXYZ(tx, ty, 0, ref cx, ref cy, ref cz); if (cy > k * cx + b) { string key = string.Format("{0},{1}", tx, ty); if (dicMon.Keys.Contains(key)) { dicMon[key]++; } else { dicMon[key] = 0; } } } } } else { for (int tx = agMinX + 1; tx < agMaxX - 1; tx++) { for (int ty = agMinY + 1; ty < agMaxY - 1; ty++) { double cx = 0, cy = 0, cz = 0; GridHelper.getInstance().AccGridToXYZ(tx, ty, 0, ref cx, ref cy, ref cz); if (cy < k * cx + b) { string key = string.Format("{0},{1}", tx, ty); if (dicMon.Keys.Contains(key)) { dicMon[key]++; } else { dicMon[key] = 0; } } } } } } #endregion } // 挑选分数最高的格子 if (dicMon.Count == 0) { return(new Result(false, "单调性未推出合理结果")); } var maxKeyMon = (from d in dicMon orderby d.Value descending select d.Key).First(); var allMaxKeyMon = (from d in dicMon where d.Value == dicMon[maxKeyMon] select d.Key).ToArray(); double X = 0, Y = 0, Z = 0; int cnt = 0; if (allMaxKeyMon.Count() > 5) { foreach (string a in allMaxKeyMon) // 有多个分数最高的格子 { string[] num = a.Split(','); GridHelper.getInstance().AccGridToXYZ(Convert.ToInt32(num[0]), Convert.ToInt32(num[1]), 0, ref X, ref Y, ref Z); Pt3D pt = new Pt3D(X, Y, 0); if (inBound(ref pt)) { MonotoneResult.Add(pt); } } } else { var dicSort = from objDic in dicMon orderby objDic.Value descending select objDic; foreach (KeyValuePair <string, int> kvp in dicSort) { string[] num = kvp.Key.Split(','); GridHelper.getInstance().AccGridToXYZ(Convert.ToInt32(num[0]), Convert.ToInt32(num[1]), 0, ref X, ref Y, ref Z); ++cnt; MonotoneResult.Add(new Pt3D(X, Y, 0)); if (cnt > 5) { break; } } } return(new Result(true)); }