// Point coordinates = new Point();
public static Point CalculatePosition(Node BlindNode, Node.FilterMethod filterMethod)
{
int anchors = 0;
Point coordinates = new Point();
List <Point> IntersectionPoints = new List <Point>();
if (BlindNode.Anchors.Count >= 3)
{
anchors = BlindNode.Anchors.Count;
for (int i = 0; i < anchors - 1; i++)
{
for (int j = i + 1; j < anchors; j++)
{
foreach (Point cuttingpoint in CuttingPoint(BlindNode.Anchors[i].posx, BlindNode.Anchors[i].posy, Ranging(BlindNode.Anchors[i].fRSS), BlindNode.Anchors[j].posx, BlindNode.Anchors[j].posy, Ranging(BlindNode.Anchors[j].fRSS)))
{
IntersectionPoints.Add(cuttingpoint);
}
}
}
return(Cluster(IntersectionPoints, anchors));
}
else
{
throw new ApplicationException("Less than three anchor nodes available");
}
}
/// <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)
{
BoundingBox BnBox, AnBox;
Point center;
center = new Point(Anchors[0].posx, Anchors[0].posy);
AnBox = new BoundingBox(center, Anchors[0].range);
BnBox = AnBox;
for (int i = 1; i < Anchors.Count; i++)
{
double distance = Anchors[i].range;
center = new Point(Anchors[i].posx, Anchors[i].posy);
AnBox = new BoundingBox(center, distance);
if (!(BnBox.Xmax < AnBox.Xmin || BnBox.Xmin > AnBox.Xmax || BnBox.Ymax < AnBox.Ymin || BnBox.Ymin > AnBox.Ymax))
{
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);
}
public static Point CalculatePosition(Node BlindNode, Node.FilterMethod filterMethod)
{
BoundingBox BnBox = new BoundingBox(0);
Point position = new Point();
double distance;
if (BlindNode.Anchors.Count >= 3)
{
foreach (AnchorNode AN in BlindNode.Anchors)
{
//perform the ranging
double fRSS = filterMethod(AN.RSS);
distance = Ranging(fRSS);
Point center = new Point(AN.posx, AN.posy);
BoundingBox 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);
}
}
else
{
throw new ApplicationException("Less than three anchor nodes available");
}
position.x = (int)(BnBox.Xmin + BnBox.Xmax) / 2;
position.y = (int)(BnBox.Ymin + BnBox.Ymax) / 2;
return(position);
}
/// <summary>
/// Calculates the position
/// </summary>
/// <param name="BlindNode">The BlindNode to be positioned</param>
/// <returns>The position of the blind node</returns>
public static Point CalculatePosition(Node BlindNode, Node.FilterMethod filterMethod)
{
Point position = new Point();
double weight;
double totalWeight = 0;
foreach (AnchorNode an in BlindNode.Anchors)
{
an.fRSS = filterMethod(an.RSS);
}
foreach (AnchorNode anchorNode in BlindNode.Anchors)
{
weight = 1 / Math.Pow(anchorNode.fRSS, 3);
position.x += anchorNode.posx * weight;
position.y += anchorNode.posy * weight;
totalWeight += weight;
}
position.x /= totalWeight;
position.y /= totalWeight;
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)
{
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);
}
public void ClusteredTriLaterationTest()
{
MySQLClass MyDB = new MySQLClass("DRIVER={MySQL ODBC 3.51 Driver};SERVER=localhost;DATABASE=senseless;UID=root;PASSWORD=admin;OPTION=3;");
Node BlindNode = new Node("Test", MyDB);
BlindNode.NewAnchor("1", 50.00, 0.00, 0.00, 1);
BlindNode.NewAnchor("2", 50.00, 2.00, 0.00, 1);
BlindNode.NewAnchor("3", 50.00, 2.00, 2.00, 1);
Node.FilterMethod filterMethod = RangeBasedPositioning.AverageFilter;
Point expected = new Point(1, 1);
Point actual;
actual = ClusterTrilateration.CalculatePosition(BlindNode, filterMethod, false);
//Assert.AreEqual(expected, actual);
Assert.Inconclusive("Verify the correctness of this test method.");
}
/// <summary>
/// Calibrates the pathloss parameter with information of the anchor nodes
/// </summary>
/// <param name="AnchorNodes">The anchor nodes giving the calibration information</param>
/// <param name="filterMethod">Method to filter the RSS</param>
public static void CalibratePathloss(List <Node> AnchorNodes, Node.FilterMethod filterMethod)
{
double pathlossExponent = 0;
//if (AnchorNodes.Count < 3)
// return;
//determine Pd0 & d0
//use default values for now...
//foreach anchornode:
foreach (Node node in AnchorNodes)
{
double tempPathLossExponent = 0.00;
//node.SetOwnPosition();
foreach (AnchorNode anchorNode in node.Anchors)
{
anchorNode.fRSS = filterMethod(anchorNode.RSS);
double distance = Math.Pow((Math.Pow((node.Position.x - anchorNode.posx), 2) + Math.Pow((node.Position.y - anchorNode.posy), 2)), 0.5);
//formula does not work when the distance is 1m, then np can not be calculated
if (distance != 1)
{
tempPathLossExponent += (anchorNode.fRSS + 51.00) / (-10 * Math.Log10(distance));
}
else
{
tempPathLossExponent += 2.00;
}
}
tempPathLossExponent /= node.Anchors.Count;
pathlossExponent += tempPathLossExponent;
}
pathlossExponent /= AnchorNodes.Count;
RangeBasedPositioning.pathLossExponent = pathlossExponent;
}
/// <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 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)
{
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>
/// Parses the data from an anchor
/// Adds the anchor to AnchorNodes
/// Updates the RSS
/// Calls the requested calibration function
/// Forms a query to add the RSS to the database
/// </summary>
/// <param name="row">Incoming data</param>
/// <returns>the SQL query to be performed</returns>
private string ParseAnchor(DataRow row)
{
Node CurrentNode;
string currentID = row["ID"].ToString();
if (!AnchorNodes.Exists(AN => AN.WsnIdProperty == currentID))
{
AnchorNodes.Add(new Node(row["ID"].ToString(), MySQLConn));
Console.WriteLine("Added new BN to be positioned\n\n\n");
}
CurrentNode = AnchorNodes.Find(AN => AN.WsnIdProperty == currentID);
CurrentNode.UpdateAnchors(row["ANode"].ToString(), Convert.ToDouble(row["RSSI"].ToString()), Convert.ToInt32(row["VANs"]), DateTime.Now);
CurrentNode.SetOwnPosition();
//CurrentNode = AnchorNodes.Find(AN => AN.WsnIdProperty == currentID);
Node.FilterMethod myFilter;
switch (SelectedFilter)
{
case "Median":
myFilter = new Node.FilterMethod(RangeBasedPositioning.MedianFilter);
break;
case "Average":
myFilter = new Node.FilterMethod(RangeBasedPositioning.AverageFilter);
break;
default:
myFilter = new Node.FilterMethod(RangeBasedPositioning.MedianFilter);
break;
}
switch (SelectedCalibration)
{
case "Normal":
RangeBasedPositioning.CalibratePathloss(AnchorNodes, myFilter);
break;
case "LeastSquares":
RangeBasedPositioning.CalibratePathlossLS(AnchorNodes, myFilter);
break;
}
int TimeSecs, tempint;
if (int.TryParse(row["Time"].ToString(), out TimeSecs))
row["Time"] = ConvertUnixToLocalTimeStamp(TimeSecs);
//Create the command that we send to the database to insert the new row.
string cmd = "call addLocalizationData(" +
((int.TryParse(row["RSSI"].ToString(), out tempint)) ? row["RSSI"] : "null") + "," +
((int.TryParse(row["X"].ToString(), out tempint)) ? row["X"] : "null") + "," +
((int.TryParse(row["Y"].ToString(), out tempint)) ? row["Y"] : "null") + "," +
((int.TryParse(row["Z"].ToString(), out tempint)) ? row["Z"] : "null") + "," +
row["ID"] + ",'" +
row["Time"] + "'," +
((int.TryParse(row["ANode"].ToString(), out tempint)) ? row["ANode"] : "null") + ",'" +
"Anchor" + "');";
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>
/// 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];
}
}
