/// <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>
/// 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)
{
List <Point> intersectionPoints = new List <Point>();
// List<AnchorNode> AllAnchors = new List<AnchorNode>();
// List<IntersectedAnchors> anchors = new List<IntersectedAnchors>();
// IntersectedAnchors anchor = new IntersectedAnchors();
// List<int> CountOfCircles = new List<int>();
Point center = new Point();
// int numberOfCircles = 0;
// foreach (AnchorNode AN in BlindNode.Anchors)
// {
// AN.fRSS = filterMethod(AN.RSS);
// AN.range = rangingMethod(AN.fRSS);
// }
// foreach (AnchorNode VAN in BlindNode.VirtualAnchors)
// {
// VAN.fRSS = filterMethod(VAN.RSS);
// VAN.range = rangingMethod(VAN.fRSS);
// }
//if (!multihop)
{
if (BlindNode.Anchors.Count >= 3)
{
for (int i = 0; i < BlindNode.Anchors.Count - 1; i++)
{
for (int j = i + 1; j < BlindNode.Anchors.Count; j++)
{
List <Point> crossingPoints =
GeometryHelper.Intersect(BlindNode.Anchors[i].posx,
BlindNode.Anchors[i].posy,
BlindNode.Anchors[i].range,
BlindNode.Anchors[j].posx,
BlindNode.Anchors[j].posy,
BlindNode.Anchors[j].range);
if (crossingPoints != null)
{
foreach (Point crossing in crossingPoints)
{
intersectionPoints.Add(crossing);
}
}
}
}
if (intersectionPoints.Count >= 3)
{
center = Cluster(intersectionPoints, BlindNode.Anchors.Count);
}
else
{
center = null;
}
}
else
{
center = null;
}
}
// else
// {
// for (int i = 0; i < BlindNode.Anchors.Count - 1; i++)
// {
// for (int j = i + 1; j < BlindNode.Anchors.Count; j++)
// {
// List<Point> crossingPoints = GeometryHelper.Intersect(BlindNode.Anchors[i].posx, BlindNode.Anchors[i].posy, BlindNode.Anchors[i].range, BlindNode.Anchors[j].posx, BlindNode.Anchors[j].posy, BlindNode.Anchors[j].range);
// if (crossingPoints != null)
// {
// foreach (Point crossing in crossingPoints)
// intersectionPoints.Add(crossing);
//
// }
// }
// }
// if (intersectionPoints.Count < 3)
// {
// intersectionPoints.Clear();
//
// foreach (AnchorNode an in BlindNode.Anchors)
// AllAnchors.Add(an);
// foreach (AnchorNode van in BlindNode.VirtualAnchors)
// AllAnchors.Add(van);
//
// for (int i = 0; i < AllAnchors.Count - 1; i++)
// {
// for (int j = i + 1; j < AllAnchors.Count; j++)
// {
// List<Point> crossingPoints = GeometryHelper.Intersect(AllAnchors[i].posx, AllAnchors[i].posy, AllAnchors[i].range, AllAnchors[j].posx, AllAnchors[j].posy, AllAnchors[j].range);
// if (crossingPoints != null)
// {
// foreach (Point crossing in crossingPoints)
// intersectionPoints.Add(crossing);
//
// }
// }
// }
// for (int i = 0; i < AllAnchors.Count; i++)
// {
// numberOfCircles = 0;
// for (int j = 0; j < AllAnchors.Count; j++)
// {
//
// if (GeometryHelper.BelongTo(AllAnchors[i].posx, AllAnchors[i].posy, AllAnchors[i].range, AllAnchors[j].posx, AllAnchors[j].posy, AllAnchors[j].range))
// numberOfCircles++;
// }
// CountOfCircles.Add(numberOfCircles);
// }
// List<int> anchorss = CountOfCircles.FindAll(number => number == 1);
//
// if (intersectionPoints.Count >= 3)
// center = Cluster(intersectionPoints, (AllAnchors.Count - anchorss.Count) );
// else
// {
// center = null;
// }
//
// }
// else
// center = Cluster(intersectionPoints, BlindNode.Anchors.Count);
//
// }
return(center);
}
/// <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 center = new Point();
List <AnchorNode> Anchors = new List <AnchorNode>();
List <AnchorNode> AllAnchors = new List <AnchorNode>();
int count = 0;
foreach (AnchorNode AN in BlindNode.Anchors)
{
AN.fRSS = filterMethod(AN.RSS);
AN.range = rangingMethod(AN.fRSS);
}
foreach (AnchorNode VAN in BlindNode.VirtualAnchors)
{
VAN.fRSS = filterMethod(VAN.RSS);
VAN.range = rangingMethod(VAN.fRSS);
}
for (int i = 0; i < BlindNode.Anchors.Count; i++)
{
count = 0;
BlindNode.Anchors[i].fRSS = filterMethod(BlindNode.Anchors[i].RSS);
for (int j = 0; j < BlindNode.Anchors.Count; j++)
{
BlindNode.Anchors[j].fRSS = filterMethod(BlindNode.Anchors[j].RSS);
if (BelongsToAllBoxes(BlindNode.Anchors[i].posx, BlindNode.Anchors[i].posy, BlindNode.Anchors[i].range, BlindNode.Anchors[j].posx, BlindNode.Anchors[j].posy, BlindNode.Anchors[j].range))
{
count++;
}
}
if (count >= 3)
{
Anchors.Add(BlindNode.Anchors[i]);
}
}
if (!multiHop)
{
if (Anchors.Count >= 3)
{
center = MinMaxCalc(Anchors, filterMethod);
}
else
{
center = null;
}
}
else
{
if (Anchors.Count >= 3)
{
center = MinMaxCalc(Anchors, filterMethod);
}
else if (Anchors.Count < 3)
{
Anchors.Clear();
foreach (AnchorNode an in BlindNode.Anchors)
{
AllAnchors.Add(an);
}
foreach (AnchorNode van in BlindNode.VirtualAnchors)
{
AllAnchors.Add(van);
}
for (int i = 0; i < AllAnchors.Count; i++)
{
count = 0;
for (int j = 0; j < AllAnchors.Count; j++)
{
if (BelongsToAllBoxes(AllAnchors[i].posx, AllAnchors[i].posy, AllAnchors[i].range, AllAnchors[j].posx, AllAnchors[j].posy, AllAnchors[j].range))
{
count++;
}
}
if (count >= 3)
{
Anchors.Add(AllAnchors[i]);
}
}
if (Anchors.Count < 3)
{
center = null;
}
else
{
center = MinMaxCalc(Anchors, filterMethod);
}
}
else
{
center = null;
}
}
return(center);
}
/// <summary>
/// Parses the blind node data
/// Adds the node to BlindNodes
/// Updates the RSS
/// Performs the requested localization algorithm
/// Forms the query to put the RSS and postion in the database
/// Possibly throws an event as described in the event section
/// </summary>
/// <param name="row"></param>
/// <param name="nodeId"></param>
/// <returns></returns>
private string ParseBlind(DataRow row, string nodeId)
{
string cmd, currentID;
int tempint;
int TimeSecs;
if (int.TryParse(row["Time"].ToString(), out TimeSecs))
row["Time"] = ConvertUnixToLocalTimeStamp(TimeSecs);
//SunSpot sends the timestamp as unix-timestamp, convert it to normal timestamp.
currentID = row["ID"].ToString();
Positioning.Point pos = new Positioning.Point(0, 0);
Node CurrentNode;
if (!BlindNodes.Exists(BN => BN.WsnIdProperty == currentID))
{
BlindNodes.Add(new Node(row["ID"].ToString(), MySQLConn));
Console.WriteLine("Added new BN to be positioned\n\n\n");
}
CurrentNode = BlindNodes.Find(BN => BN.WsnIdProperty == currentID);
CurrentNode.UpdateAnchors(row["ANode"].ToString(), Convert.ToDouble(row["RSSI"].ToString()), Convert.ToInt32(row["VANs"]), DateTime.Now);
//TODO: check if automatically updated
CurrentNode = BlindNodes.Find(BN => BN.WsnIdProperty == currentID);
Node.FilterMethod myFilter = new Node.FilterMethod(RangeBasedPositioning.MedianFilter);;
Node.RangingMethod myRanging;
if (SelectedCalibration != "Disabled")
myRanging = new Node.RangingMethod(RangeBasedPositioning.Ranging);
else
myRanging = new Node.RangingMethod(RangeBasedPositioning.DefaultRanging);
switch (SelectedFilter)
{
case "Median":
myFilter = new Node.FilterMethod(RangeBasedPositioning.MedianFilter);
break;
case "Average":
myFilter = new Node.FilterMethod(RangeBasedPositioning.AverageFilter);
break;
case "NoFilter":
myFilter = new Node.FilterMethod(RangeBasedPositioning.NoFilter);
break;
}
switch (SelectedAlgorithm)
{
case "CentroidLocalization":
//pos = CentroidLocalization.CalculatePosition(CurrentNode);
pos = CentroidLocalization.CalculatePosition(CurrentNode);
break;
case "MinMax":
pos = MinMax.CalculatePosition(CurrentNode, myFilter, myRanging, UseMultihop);
break;
case "Trilateration":
pos = ExtendedTrilateration.CalculatePosition(CurrentNode, myFilter, myRanging, UseMultihop);
break;
case "ExtendedTrilateration":
pos = ExtendedTrilateration.CalculatePosition(CurrentNode, myFilter, myRanging, UseMultihop);
break;
case "ExtendedMinMax":
pos = MinMaxExtended.CalculatePosition(CurrentNode, myFilter, myRanging, UseMultihop);
break;
case "WeightedCentroidLocalization":
pos = WCL.CalculatePosition(CurrentNode, myFilter);
break;
case "LSTrilateration":
pos = LSTrilateration.CalculatePosition(CurrentNode, myFilter, myRanging, UseMultihop);
break;
}
if (pos != null)
Console.WriteLine("Position was calculated for node: " + row["ID"] + " X = " + pos.x.ToString() + " Y = " + pos.y.ToString());
else
{
Console.WriteLine("Position for node: " + row["ID"] + " could not be calculated");
if (CurrentNode.Anchors.Count < 3)
Console.WriteLine("Reason: too few anchor nodes!: " + CurrentNode.Anchors.Count.ToString());
else
Console.WriteLine("Unspecified error");
}
//Create the command that we send to the database to insert the new row.
cmd = "call addLocalizationData(" +
((int.TryParse(row["RSSI"].ToString(), out tempint)) ? row["RSSI"] : "null") + ",";
if (pos != null)
cmd += pos.x.ToString().Replace(',','.') + ", " + pos.y.ToString().Replace(',','.') + ", ";
else
cmd += "null, null, ";
cmd += ((int.TryParse(row["Z"].ToString(), out tempint)) ? row["Z"] : "null") + "," +
row["ID"] + ",'" +
row["Time"] + "'," +
((int.TryParse(row["ANode"].ToString(), out tempint)) ? row["ANode"] : "null") + ",'" +
SelectedAlgorithm + "');";
//add the position to the position table
AddPosition(row, pos, 0);
#region LocationUpdated
if (LocationUpdated != null && pos != null)
{
EventMessage EventData = new EventMessage();
//EventData.EventType = "LocationUpdated";
EventData.TagBlink["TagID"] = nodeId;
EventData.TagBlink["Accuracy"] = WsnEngine.CheckMapBounds(ref pos.x, ref pos.y, "2");
EventData.TagBlink["MapID"] = "WsnEngine1map2";
EventData.TagBlink["X"] = pos.x.ToString();
EventData.TagBlink["Y"] = pos.y.ToString();
LocationUpdated(this, EventData);
}
#endregion
return cmd;
}
/// <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)
{
List <Point> intersectionPoints = new List <Point>();
Point position = new Point();
List <string> StatusCircles = new List <string>();
List <int> ListOfCounts = new List <int>();
int count;
bool AllCirclesIntersected = false;
// foreach (AnchorNode an in BlindNode.Anchors)
// {
// an.fRSS = filterMethod(an.RSS);
// an.range = rangingMethod(an.fRSS);
// }
if (!multihop)
{
if (BlindNode.Anchors.Count >= 3)
{
// This lus will continue with expanding and narrowing the range of the anchors
// until all circles cut in one or two points
while (!AllCirclesIntersected)
{
for (int i = 0; i < BlindNode.Anchors.Count; i++)
{
count = 0;
for (int j = 0; j < BlindNode.Anchors.Count; j++)
{
StatusCircles.Add(GeometryHelper.InOrOut(BlindNode.Anchors[i].posx,
BlindNode.Anchors[i].posy,
BlindNode.Anchors[i].range,
BlindNode.Anchors[j].posx,
BlindNode.Anchors[j].posy,
BlindNode.Anchors[j].range));
}
if (StatusCircles.Contains("Overlap") && !(StatusCircles.Contains("In") || StatusCircles.Contains("Separated")))
{
BlindNode.Anchors[i].range *= 0.9;
}
else if ((StatusCircles.Contains("In") || StatusCircles.Contains("Separated")) && !StatusCircles.Contains("Overlap"))
{
BlindNode.Anchors[i].range *= 1.1;
}
StatusCircles.Clear();
}
// Check if all nodes intersect
ListOfCounts.Clear();
for (int i = 0; i < BlindNode.Anchors.Count; i++)
{
count = 0;
for (int j = 0; j < BlindNode.Anchors.Count; j++)
{
if (GeometryHelper.BelongTo(BlindNode.Anchors[i].posx, BlindNode.Anchors[i].posy, BlindNode.Anchors[i].range, BlindNode.Anchors[j].posx, BlindNode.Anchors[j].posy, BlindNode.Anchors[j].range))
{
count++;
}
}
ListOfCounts.Add(count);
}
if (ListOfCounts.Average() == BlindNode.Anchors.Count)
{
AllCirclesIntersected = true;
}
}
// Determine crossings
for (int i = 0; i < BlindNode.Anchors.Count - 1; i++)
{
for (int j = i + 1; j < BlindNode.Anchors.Count; j++)
{
//returns 0, 1 or 2 Pointss
List <Point> crossingPoints = GeometryHelper.Intersect(BlindNode.Anchors[i].posx, BlindNode.Anchors[i].posy, BlindNode.Anchors[i].range, BlindNode.Anchors[j].posx, BlindNode.Anchors[j].posy, BlindNode.Anchors[j].range);
if (crossingPoints != null)
{
foreach (Point crossing in crossingPoints)
{
intersectionPoints.Add(crossing);
}
}
}
}
// Calculate BN position with clustering
if (intersectionPoints.Count >= 3)
{
position = Cluster(intersectionPoints, BlindNode.Anchors.Count);
}
else
{
position = null;
}
}
else
{
position = null;
}
}
return(position);
}
/// <summary>
/// Calculates the intersection points between two circles
/// </summary>
/// <param name="Anchors">List of anchor nodes</param>
/// <param name="filterMethod">The filter to use on the RSS values</param>
/// <returns>The center of the box in common</returns>
public static Point MinMaxCalc(List <AnchorNode> Anchors, Node.FilterMethod filterMethod, Node.RangingMethod rangingMethod)
{
BoundingBox BnBox, AnBox;
double distance = Anchors[0].range;
Point center;
center = new Point(Anchors[0].posx, Anchors[0].posy);
AnBox = new BoundingBox(center, distance);
BnBox = AnBox;
for (int i = 1; i < Anchors.Count; i++)
{
distance = Anchors[i].range;
center = new Point(Anchors[i].posx, Anchors[i].posy);
AnBox = new BoundingBox(center, distance);
BnBox.Xmin = Math.Max(BnBox.Xmin, AnBox.Xmin);
BnBox.Xmax = Math.Min(BnBox.Xmax, AnBox.Xmax);
BnBox.Ymin = Math.Max(BnBox.Ymin, AnBox.Ymin);
BnBox.Ymax = Math.Min(BnBox.Ymax, AnBox.Ymax);
}
center.x = (BnBox.Xmin + BnBox.Xmax) / 2;
center.y = (BnBox.Ymin + BnBox.Ymax) / 2;
return(center);
}
/// <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 center = new Point();
List <AnchorNode> Anchors = new List <AnchorNode>();
List <AnchorNode> AllAnchors = new List <AnchorNode>();
List <int> ListOfCounts = new List <int>();
int count = 0;
bool AllBoxesIntersected = false;
foreach (AnchorNode an in BlindNode.Anchors)
{
an.fRSS = filterMethod(an.RSS);
double frss = rangingMethod(an.fRSS);
an.range = frss;
}
if (!multiHop)
{
while (!AllBoxesIntersected)
{
for (int i = 0; i < BlindNode.Anchors.Count; i++)
{
count = 0;
for (int j = 0; j < BlindNode.Anchors.Count; j++)
{
if (BelongsToAllBoxes(BlindNode.Anchors[i].posx, BlindNode.Anchors[i].posy, BlindNode.Anchors[i].range, BlindNode.Anchors[j].posx, BlindNode.Anchors[j].posy, BlindNode.Anchors[j].range))
{
count++;
}
}
ListOfCounts.Add(count);
}
if (ListOfCounts.Average() != BlindNode.Anchors.Count)
{
ListOfCounts.Clear();
foreach (AnchorNode an in BlindNode.Anchors)
{
an.range *= 1.1;
}
}
else
{
AllBoxesIntersected = true;
}
}
if (BlindNode.Anchors.Count >= 3)
{
center = MinMaxCalc(BlindNode.Anchors, filterMethod, rangingMethod);
}
else
{
center = null;
}
}
return(center);
}
