Esempio n. 1
0
        /// <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);
        }
Esempio n. 2
0
        /// <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];
            }
        }