/// <summary> /// Give any 3 valid anchors then return the x, y position /// </summary> private bool loc_trilateration(class_Anchors anchorA, class_Anchors anchorB, class_Anchors anchorC) { Single x, y, x1, y1, x2, y2, x3, y3; Single ca, cb, cc, cd, ce, cf, cg, ch, ci; bool v1, v2, v3; short count; // calculate trilateration ca = anchorB.X - anchorA.X; cb = anchorB.Y - anchorA.Y; cc = (Single)(Math.Pow(anchorA.Range, 2) - Math.Pow(anchorB.Range, 2) - Math.Pow(anchorA.X, 2) + Math.Pow(anchorB.X, 2) - Math.Pow(anchorA.Y, 2) + Math.Pow(anchorB.Y, 2)); cd = anchorB.X - anchorC.X; ce = anchorB.Y - anchorC.Y; cf = (Single)(Math.Pow(anchorC.Range, 2) - Math.Pow(anchorB.Range, 2) - Math.Pow(anchorC.X, 2) + Math.Pow(anchorB.X, 2) - Math.Pow(anchorC.Y, 2) + Math.Pow(anchorB.Y, 2)); cg = anchorA.X - anchorC.X; ch = anchorA.Y - anchorC.Y; ci = (Single)(Math.Pow(anchorC.Range, 2) - Math.Pow(anchorA.Range, 2) - Math.Pow(anchorC.X, 2) + Math.Pow(anchorA.X, 2) - Math.Pow(anchorC.Y, 2) + Math.Pow(anchorA.Y, 2)); if (cd * cb == ca * ce) { x1 = 0; y1 = 0; v1 = false; } else { y1 = 0.5f * (cd * cc - ca * cf) / (cd * cb - ca * ce); if (ca == 0) x1 = (cf - 2 * ce * y1) / (2 * cd); else x1 = (cc - 2 * cb * y1) / (2 * ca); v1 = true; } if (cg * cb == ca * ch) { x2 = 0; y2 = 0; v2 = false; } else { y2 = 0.5f * (cg * cc - ca * ci) / (cg * cb - ca * ch); if (ca == 0) x2 = (ci - 2 * ch * y2) / (2 * cg); else x2 = (cc - 2 * cb * y2) / (2 * ca); v2 = true; } if (cg * ce == cd * ch) { x3 = 0; y3 = 0; v3 = false; } else { y3 = 0.5f * (cg * cf - cd * ci) / (cg * ce - cd * ch); if (cd == 0) x3 = (ci - 2 * ch * y3) / (2 * cg); else x3 = (cf - 2 * ce * y3) / (2 * cd); v3 = true; } x = 0; y = 0; count = 0; if (v1) { x = x + x1; y = y + y1; count++; } if (v2) { x = x + x2; y = y + y2; count++; } if (v3) { x = x + x3; y = y + y3; count++; } if (count > 0) { x = x / count; y = y / count; } else return false; myTag.X = x; myTag.Y = y; myTag.Avg.X = x; myTag.Avg.Y = y; for (count = 0; count < myTag_Old_X.Length; count++) { myTag_Old_X[count] = x; myTag_Old_Y[count] = y; } return true; }
private bool loc_initial() { class_Anchors[] candidate3 = new class_Anchors[3]; Single[] tmpSingleA = new Single[6]; int anchor_count = 0; for (i = 0; i < 3; i++) { candidate3[i] = anchor[i]; } for (i = 0; i < anchor.Count; i++) { if (anchor[i].Reliability >= candidate3[0].Reliability) { candidate3[2] = candidate3[1]; candidate3[1] = candidate3[0]; candidate3[0] = anchor[i]; anchor_count++; continue; } else if (anchor[i].Reliability >= candidate3[1].Reliability) { candidate3[2] = candidate3[1]; candidate3[1] = anchor[i]; anchor_count++; continue; } else if (anchor[i].Reliability >= candidate3[2].Reliability) { candidate3[2] = anchor[i]; anchor_count++; continue; } } //Log.Debug("Brian", "count= " + anchor_count.ToString()); if (anchor_count < 3) { Log.Debug(Tag, "Searching for beacon...\r\n"); bBeaconFind = false; return false; } else { if (!loc_trilateration(candidate3[0], candidate3[1], candidate3[2])) { return false; } Log.Debug("Brian", "Beacon found!!"); bBeaconFind = true; myEKF = new class_EKFL5(myTag.X, myTag.Y, 0f, anchor.Count); for (i = 0; i < anchor.Count; i++) { myEKF.Anchor[i] = anchor[i]; } return true; } }
private bool ReadConfig(string ff) { if (File.Exists(ff)) { StreamReader sr = new StreamReader(ff); int tmpInt, count1 = 0; string line; string[] linesplit; //var adapter = new ArrayAdapter<string>(this, Android.Resource.Layout.SimpleSpinnerDropDownItem); //adapter.Add("Favorite"); while (!sr.EndOfStream) { line = sr.ReadLine(); linesplit = line.Split('='); if (linesplit[0] == "mapwidth") cfg.MapWidth = Convert.ToUInt16(linesplit[1]); else if (linesplit[0] == "mapheight") cfg.MapHeight = Convert.ToUInt16(linesplit[1]); else if (linesplit[0] == "mapeast") cfg.MapEast = Convert.ToInt16(linesplit[1]); else if (linesplit[0] == "gridwidth") cfg.GridWidth = Convert.ToUInt16(linesplit[1]); else if (linesplit[0] == "gridheight") cfg.GridHeight = Convert.ToUInt16(linesplit[1]); else if (linesplit[0] == "agent_max_speed") cfg.MaxSpeed = Convert.ToUInt16(linesplit[1]); else if (linesplit[0] == "anchor") { class_Anchors tmpanchor = new class_Anchors(Convert.ToInt32(linesplit[1]), Convert.ToSingle(linesplit[2]), Convert.ToSingle(linesplit[3]), Convert.ToSingle(linesplit[4])); anchor.Add(tmpanchor); } else if (linesplit[0] == "favorite") { //favorite[count1].Note = linesplit[1]; //favorite[count1].X = Convert.ToInt16(linesplit[2]); //favorite[count1].Y = Convert.ToInt16(linesplit[3]); //adapter.Add(favorite[count1].Note); count1++; } } sr.Close(); //spinnerTarget.Adapter = adapter; Log.Debug("Brian", "anchor num=" + anchor.Count.ToString()); return true; } else return false; }