/// <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];
}
}
/// <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>Solve X*A = B, which is also A'*X' = B'</summary>
/// <param name="B"> right hand side
/// </param>
/// <returns> solution if A is square, least squares solution otherwise.
/// </returns>
public virtual GeneralMatrix SolveTranspose(GeneralMatrix B)
{
return(Transpose().Solve(B.Transpose()));
}
/// <summary>Solve X*A = B, which is also A'*X' = B'</summary>
/// <param name="B"> right hand side
/// </param>
/// <returns> solution if A is square, least squares solution otherwise.
/// </returns>
public virtual GeneralMatrix SolveTranspose(GeneralMatrix B)
{
return Transpose().Solve(B.Transpose());
}
/// <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];
}
}
