Exemple #1
0
        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);
        }
Exemple #2
0
        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);
        }