Accuracy Analysis algorithm

edit

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 ...