Accuracy Analysis algorithm

asked 2019-12-10 06:29:44 -0600

jiujiu-ella gravatar image

updated 2019-12-10 07:11:15 -0600

Algorithm:

def calc_accuracy(tag,
                      anch_a,
                      anch_b,
                      anch_s,
                      sig_c):
        """
        This function calculates the DRMS(Distance Root Mean Square) value of
        position.
        """

        # Given coordinates
        # T = np.zeros((1,3), dtype=float)
        tag_x = tag[0]
        tag_y = tag[1]
        tag_z = tag[2]
        tmp_tag = np.array(tag)

        # A = np.zeros((1,3), dtype=float)
        a_x = anch_a[0]
        a_y = anch_a[1]
        # a_z = anch_a[2]     # UNUSED
        tmp_anch_a = np.array(anch_a)

        # B = np.zeros((1,3), dtype=float)
        b_x = anch_b[0]
        b_y = anch_b[1]
        # b_z = anch_b[2]     # UNUSED
        tmp_anch_b = np.array(anch_b)

        # S = np.zeros((1,3), dtype=float)
        s_x = anch_s[0]
        s_y = anch_s[1]
        s_z = anch_s[2]
        tmp_anch_s = np.array(anch_s)

        # see eq.(3), eq.(4) and eq.(5) in document "Annex_II_2D_Positioning"
        radius_s = LA.norm(tmp_tag-tmp_anch_s)
        radius_a = LA.norm(tmp_tag-tmp_anch_a)
        radius_b = LA.norm(tmp_tag-tmp_anch_b)

        # see eq.(1) in document "Annex_II_2D_Positioning"
        das = radius_a-radius_s
        dbs = radius_b-radius_s

        # see eq.(7) in document "Annex_IV_Accuracy Analysis"
        matrix_h = np.zeros((2, 2), dtype=float)
        matrix_h[0][0] = float(2*(a_x-s_x))
        matrix_h[0][1] = float(2*(a_y-s_y))
        matrix_h[1][0] = float(2*(b_x-s_x))
        matrix_h[1][1] = float(2*(b_y-s_y))

        # rank of the H matrix is ??controlled
        if matrix_rank(matrix_h) < 2:
            drms = float("inf")
        else:
            # see eq.(8) in document "Annex_IV_Accuracy Analysis"
            # K is pseudo inverse of matrix H
            matrix_k = np.zeros((2, 2), dtype=float)

            # K = ( H.transpose() * H ) \ ( H.transpose() )
            matrix_k = LA.pinv(matrix_h)

            k11 = matrix_k[0][0]
            k12 = matrix_k[0][1]
            k21 = matrix_k[1][0]
            k22 = matrix_k[1][1]

            # print("k11: " + str(k11))
            # print("k12: " + str(k12))
            # print("k21: " + str(k21))
            # print("k22: " + str(k22))

            anch_s = np.array([s_x, s_y, s_z])

            # initial condition and threshold value for loop
            sig_r = 0
            flag = 1
            thr = 0.5

            while flag == 1:
                # see eq.(20) in document "Annex_IV_Accuracy Analysis"
                x11 = 4*pow(das, 2)*pow(sig_r, 2) + \
                    8*das*(radius_s+das)*sig_r*sig_c + \
                    4*pow((radius_s+das), 2)*pow(sig_c, 2)

                x12 = 4*das*dbs*pow(sig_r, 2) + 4*das*radius_s*sig_r*sig_c + \
                    8*das*dbs*sig_r*sig_c + 4*dbs*radius_s*sig_c*sig_r

                x21 = x12

                x22 = 4*pow(dbs, 2)*pow(sig_r, 2) + 8*dbs*(radius_s+dbs)*sig_r*sig_c + \
                    4*pow((radius_s+dbs), 2)*pow(sig_c, 2)

                # see eq.(24), eq.(25) and eq(26) in document "Annex_IV_Accuracy Analysis"
                # sig_x = sqrt(pow(k11, 2)*x11 + 2*k11*k12*x12 + pow(k12, 2)*x22)
                # sig_y = sqrt(pow(k21, 2)*x11 + 2*k21*k22*x12 + pow(k22, 2)*x22)

                try:
                    sig_x = sqrt(pow(k11, 2)*x11 + 2*k11*k12*x12 + pow(k12, 2)*x22)
                except ValueError: # as err:
                    print "Gx ValueError"
                    return -1

                try:
                    sig_y = sqrt(pow(k21, 2)*x11 + 2*k21*k22*x12 + pow(k22, 2)*x22)
                except ValueError: # as err:
                    print "Gy ValueError"
                    return -1

                sig_x_sig_y = (k11*x11+k12*x21)*k21 + (k11*x12+k12*x22)*k22

                # see Step 9 in iterative solution in document "Annex_IV_Accuracy Analysis"
                if sig_x_sig_y >= 0:
                    p_2 = np.array([tag_x + sig_x/2, tag_y + sig_y/2, tag_z])
                    p_1 = np.array([tag_x - sig_x/2, tag_y - sig_y/2, tag_z])
                else:
                    p_2 = np.array([tag_x - sig_x/2, tag_y + sig_y/2, tag_z])
                    p_1 = np.array([tag_x ...
(more)
edit retag flag offensive close merge delete

Comments

1

So... what's your question?

Martin Günther gravatar imageMartin Günther ( 2019-12-10 07:11:41 -0600 )edit

I want to know the detailed Accuracy Analysis algorithm derivation because I want to do 3D calculations.Do you have relevant information? Greatful

jiujiu-ella gravatar imagejiujiu-ella ( 2019-12-10 07:28:34 -0600 )edit
2

Please edit your question and add more context. I have no idea what you are talking about.

Martin Günther gravatar imageMartin Günther ( 2019-12-10 07:31:26 -0600 )edit

I am doing research on indoor positioning of objects. The method of selecting an appropriate anchor point with reference to GPS positioning, that is, calculating the minimum PDOP (Position Dilution of Precision)value. The code of this calculation process is what I show. But I don't understand the loop of this program. can you help me?

jiujiu-ella gravatar imagejiujiu-ella ( 2019-12-10 07:38:33 -0600 )edit

sig_c is Standard deviation of distance measurement about TDOA

jiujiu-ella gravatar imagejiujiu-ella ( 2019-12-10 07:42:36 -0600 )edit