/// <summary> /// Calculates the position /// </summary> /// <param name="BlindNode">The BlindNode to be positioned</param> /// <param name="filterMethod">The filter to use on the RSS values</param> /// <param name="RangingMethod">The ranging method</param> /// <param name="multihop">use multihop or not</param> /// <returns>The position of the blind node</returns> public static Point CalculatePosition(Node BlindNode, Node.FilterMethod filterMethod, Node.RangingMethod rangingMethod, bool multihop) { Point position = new Point(); List <AnchorNode> AllAnchors = new List <AnchorNode>(); double[][] y = new double[BlindNode.Anchors.Count - 1][]; double[][] x = new double[BlindNode.Anchors.Count - 1][]; foreach (AnchorNode an in BlindNode.Anchors) { an.fRSS = filterMethod(an.RSS); an.range = rangingMethod(an.fRSS); } if (!multihop) { if (BlindNode.Anchors.Count >= 3) { for (int i = 1; i < BlindNode.Anchors.Count; i++) { y[i - 1] = new double[] { Math.Pow(BlindNode.Anchors[i].posx, 2) - Math.Pow(BlindNode.Anchors[0].posx, 2) + Math.Pow(BlindNode.Anchors[i].posy, 2) - Math.Pow(BlindNode.Anchors[0].posy, 2) - Math.Pow(BlindNode.Anchors[i].range, 2) + Math.Pow(BlindNode.Anchors[0].range, 2) }; x[i - 1] = new double[] { BlindNode.Anchors[i].posx - BlindNode.Anchors[0].posx, BlindNode.Anchors[i].posy - BlindNode.Anchors[0].posy }; } } else { position = null; } } else { foreach (AnchorNode an in BlindNode.Anchors) { AllAnchors.Add(an); } foreach (AnchorNode van in BlindNode.VirtualAnchors) { AllAnchors.Add(van); } for (int i = 1; i < AllAnchors.Count; i++) { if (AllAnchors[i].posx == AllAnchors[0].posx) { AllAnchors[i].posx += 0.1; } if (AllAnchors[i].posy == AllAnchors[0].posy) { AllAnchors[i].posy += 0.1; } y[i - 1] = new double[] { Math.Pow(AllAnchors[i].posx, 2) - Math.Pow(AllAnchors[0].posx, 2) + Math.Pow(AllAnchors[i].posy, 2) - Math.Pow(AllAnchors[0].posy, 2) - Math.Pow(AllAnchors[i].range, 2) + Math.Pow(AllAnchors[0].range, 2) }; x[i - 1] = new double[] { AllAnchors[i].posx - AllAnchors[0].posx, AllAnchors[i].posy - AllAnchors[0].posy }; } } GeneralMatrix Y = new GeneralMatrix(y); GeneralMatrix X = new GeneralMatrix(x); GeneralMatrix XT = X.Transpose(); GeneralMatrix haakjes = XT.Multiply(X); GeneralMatrix inverted = haakjes.Inverse(); // 2 * 2 GeneralMatrix XTY = XT.Multiply(Y); // 2 * 1 GeneralMatrix sol = inverted.Multiply(XTY); GeneralMatrix SOL2 = sol.Multiply(0.5); position.x = SOL2.Array[0][0]; position.y = SOL2.Array[1][0]; return(position); }
/// <summary> /// Calibrates the pathloss parameter with information of the anchor nodes using Least Squares /// </summary> /// <param name="AnchorNodes">The anchor nodes giving the calibration information</param> /// <param name="filterMethod">Method to filter the RSS</param> public static void CalibratePathlossLS(List <Node> CalibrationNodes, Node.FilterMethod filterMethod) { double pathlossExponent = 0; List <Node> AllAnchors = new List <Node>(); TwoAnchors twoAnchors1 = new TwoAnchors(); TwoAnchors twoAnchors2 = new TwoAnchors(); List <TwoAnchors> AllCalAnchors = new List <TwoAnchors>(); AllAnchors = CalibrationNodes; for (int j = 0; j < CalibrationNodes.Count; j++) { twoAnchors1.a1 = CalibrationNodes[j].WsnId; twoAnchors2.a2 = CalibrationNodes[j].WsnId; //CalibrationNodes[j].SetOwnPosition(); for (int i = 0; i < CalibrationNodes[j].Anchors.Count; i++) { twoAnchors1.a2 = CalibrationNodes[j].Anchors[i].nodeid; twoAnchors2.a1 = CalibrationNodes[j].Anchors[i].nodeid; if (!AllCalAnchors.Contains(twoAnchors1) && !AllCalAnchors.Contains(twoAnchors2)) { AllCalAnchors.Add(twoAnchors1); AllCalAnchors.Add(twoAnchors2); } else { foreach (Node mote in AllAnchors) { if (mote.WsnId == CalibrationNodes[j].Anchors[i].nodeid) { foreach (AnchorNode an in mote.Anchors) { if (an.nodeid == CalibrationNodes[j].WsnId) { foreach (double d in CalibrationNodes[j].Anchors[i].RSS) { an.RSS.Enqueue(d); } // mote.Anchors.Remove(CalibrationNodes[j].Anchors[i]); } } } } foreach (Node mote in AllAnchors) { if (mote.WsnId == CalibrationNodes[j].WsnId) { mote.Anchors.Remove(CalibrationNodes[j].Anchors[i]); } } } } } int totalcountt = 0; foreach (Node nod in AllAnchors) { totalcountt += nod.Anchors.Count; } if (totalcountt >= 3) { int totalcount = 0; int count = 0; foreach (Node node in AllAnchors) { totalcount += node.Anchors.Count; } double[][] y = new double[totalcount][]; double[][] x = new double[totalcount][]; foreach (Node cal in AllAnchors) { for (int i = 0; i < cal.Anchors.Count; i++) { cal.Anchors[i].fRSS = filterMethod(cal.Anchors[i].RSS); double distance = Math.Pow((Math.Pow((cal.Position.x - cal.Anchors[i].posx), 2) + Math.Pow((cal.Position.y - cal.Anchors[i].posy), 2)), 0.5); if (distance == 0) { distance = 0.1; } y[count] = new double[1] { cal.Anchors[i].fRSS }; x[count] = new double[2] { 1, -10 * Math.Log10(distance) }; count++; } } GeneralMatrix Y = new GeneralMatrix(y); GeneralMatrix X = new GeneralMatrix(x); GeneralMatrix XT = X.Transpose(); GeneralMatrix haakjes = XT.Multiply(X); GeneralMatrix inverted = haakjes.Inverse(); GeneralMatrix XTY = XT.Multiply(Y); GeneralMatrix sol = inverted.Multiply(XTY); RangeBasedPositioning.baseLoss = -sol.Array[0][0]; RangeBasedPositioning.pathLossExponent = sol.Array[1][0]; } }