# Accuracy Analysis algorithm 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
tag_y = tag
tag_z = tag
tmp_tag = np.array(tag)

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

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

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

# see eq.(3), eq.(4) and eq.(5) in document "Annex_II_2D_Positioning"

# see eq.(1) in document "Annex_II_2D_Positioning"

# see eq.(7) in document "Annex_IV_Accuracy Analysis"
matrix_h = np.zeros((2, 2), dtype=float)
matrix_h = float(2*(a_x-s_x))
matrix_h = float(2*(a_y-s_y))
matrix_h = float(2*(b_x-s_x))
matrix_h = 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
k12 = matrix_k
k21 = matrix_k
k22 = matrix_k

# 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) + \

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

x21 = x12

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

# 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 ...
edit retag close merge delete