public List <TagLoc> Trilaterate3D()
{
List <TagLoc> LocList = new List <TagLoc>();
foreach (Tag tag in m_Tags)
{
TagLoc loc = tag.Trilaterate3D();
if (loc != null)
{
LocList.Add(loc);
}
}
return(LocList);
}
public TagLoc Trilaterate3D()
{
TagLoc tloc = new TagLoc(Id);
//.. 1. Session'u local time'a göre büyükten küçüğe sırala
m_Sessions.Sort((x, y) => - 1 * x.TimeLocal.CompareTo(y.TimeLocal)); //.. büyükten küçüğe sıralar
//.. 2. En yeniden en eskiye session'daki Anchor slot sayısı yeterli sayıdaysa
for (int s = 0; s < m_Sessions.Count; s++)
{
Session session = m_Sessions.ElementAt(s);
if (session.blProcessed)
{
continue;
}
if (session.m_Tdoas.Count >= MIN_SAMPLE_FOR_2D)
{
Matrix <double> A = new DenseMatrix(session.m_Tdoas.Count - 1, 4);
Matrix <double> b = new DenseMatrix(session.m_Tdoas.Count - 1, 1);
UInt32 SessionId = session.Id;
Tdoa a0 = session.m_Tdoas.ElementAt(0);
for (int i = 0; i < A.RowCount; i++)
{
Tdoa ai = session.m_Tdoas.ElementAt(i + 1);
A[i, 0] = a0.Anchor.x - ai.Anchor.x;
A[i, 1] = a0.Anchor.y - ai.Anchor.y;
A[i, 2] = a0.Anchor.z - ai.Anchor.z;
A[i, 3] = ((double)UnitDiff((Int64)a0.Time, (Int64)ai.Time)) * root.UnitInCentimeter;
b[i, 0] = 0.5 * (Math.Pow(a0.Anchor.x, 2) - Math.Pow(ai.Anchor.x, 2) +
Math.Pow(a0.Anchor.y, 2) - Math.Pow(ai.Anchor.y, 2) +
Math.Pow(a0.Anchor.z, 2) - Math.Pow(ai.Anchor.z, 2) +
Math.Pow(A[i, 3], 2));
}
/*Matrix<double> Xls = LeastSquare(A, b);
* if (Xls != null) {
* tloc.AddLoc(SessionId, session.TimeLocal, Xls[0, 0], Xls[1, 0], Xls[2, 0]);
* }*/
Matrix <double> Svd = SingulerValueDecomposition(A, b);
if (Svd != null)
{
tloc.AddLoc(SessionId, session.TimeLocal, Svd[0, 0], Svd[1, 0], Svd[2, 0]);
}
/*Matrix<double> Qrd = QRDecomposition(A, b);
* if (Qrd != null) {
* tloc.AddLoc(SessionId, session.TimeLocal, Qrd[0, 0], Qrd[1, 0], Qrd[2, 0]);
* }*/
session.blProcessed = true;
}
}
return(tloc);
}
