def TWLSHDFLocate(self, RN_RSS, RN_ToA, RN_TDoA, RN_TDoA2, ToA, ToAStd,
TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest):
"""
This applies LS approximation to get position P.
Return P
"""
c = 3e08
RSSL = RSSLocation(RN_RSS)
if RN_RSS == None:
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA * RN_ToA, axis=0)).reshape(RNnum_ToA, 1)
RoA = c * ToA
RoAStd = c * ToAStd
RoA2 = (RoA * RoA).reshape(RNnum_ToA, 1)
K_ToA = RN_ToA2[1:RNnum_ToA, :] - RN_ToA2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_ToA, :]
A_ToA = hstack(
(RN_ToA[:, 1:RNnum_ToA].T -
RN_ToA[:, 0].reshape(1, shRN_ToA[0]), zeros(
(RNnum_ToA - 1, 1))))
C_ToA = RoAStd[1:RNnum_ToA, 0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_ToA[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c * TDoA
RDoAStd = c * TDoAStd
RDoA2 = (RDoA * RDoA).reshape(RNnum_TDoA, 1)
K_TDoA = (sum(
(RN_TDoA - RN_TDoA2) *
(RN_TDoA - RN_TDoA2), axis=0)).reshape(RNnum_TDoA, 1) - RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T, 0.5 * RoA[0, 0] * RDoA))
C_TDoA = RDoAStd[:, 0]**2
# solution
K = vstack((K_ToA, K_TDoA))
A = vstack((A_ToA, A_TDoA))
C = diag(hstack((C_ToA, C_TDoA)))
A2 = dot(A.T, dot(linalg.inv(C), A))
[U, S, V] = svd(A2)
J = 1 / S
rA = rank(A)
m, n = shape(A)
f = 0
if log10(cond(A2)) >= max(
RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)):
f = f + 1
for i in range(n - rA):
u = where(J == max(J))
J[u] = 0
A2i = dot(dot(V.T, diag(J)), U.T)
P = 0.5 * dot(A2i, dot(dot(A.T, linalg.inv(C)), K))
return P[:2, :]
elif RN_ToA == None:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS * RN_RSS, axis=0)).reshape(RNnum_RSS, 1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)
RoA2 = (RoA * RoA).reshape(RNnum_RSS, 1)
K_RSS = RN_RSS2[1:RNnum_RSS, :] - RN_RSS2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_RSS, :]
A_RSS = hstack(
(RN_RSS[:, 1:RNnum_RSS].T -
RN_RSS[:, 0].reshape(1, shRN_RSS[0]), zeros(
(RNnum_RSS - 1, 1))))
C_RSS = RoAStd[1:RNnum_RSS, 0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_RSS[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c * TDoA
RDoAStd = c * TDoAStd
RDoA2 = (RDoA * RDoA).reshape(RNnum_TDoA, 1)
K_TDoA = (sum(
(RN_TDoA - RN_TDoA2) *
(RN_TDoA - RN_TDoA2), axis=0)).reshape(RNnum_TDoA, 1) - RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T, 0.5 * RoA[0, 0] * RDoA))
C_TDoA = RDoAStd[:, 0]**2
# solution
K = vstack((K_RSS, K_TDoA))
A = vstack((A_RSS, A_TDoA))
C = diag(hstack((C_RSS, C_TDoA)))
A2 = dot(A.T, dot(linalg.inv(C), A))
[U, S, V] = svd(A2)
J = 1 / S
rA = rank(A)
m, n = shape(A)
f = 0
if log10(cond(A2)) >= max(
RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)):
f = f + 1
for i in range(n - rA):
u = where(J == max(J))
J[u] = 0
A2i = dot(dot(V.T, diag(J)), U.T)
P = 0.5 * dot(A2i, dot(dot(A.T, linalg.inv(C)), K))
return P[:2, :]
elif RN_TDoA == None:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS * RN_RSS, axis=0)).reshape(RNnum_RSS, 1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)
RoA2 = (RoA * RoA).reshape(RNnum_RSS, 1)
K_RSS = RN_RSS2[1:RNnum_RSS, :] - RN_RSS2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_RSS, :]
A_RSS = hstack(
(RN_RSS[:, 1:RNnum_RSS].T -
RN_RSS[:, 0].reshape(1, shRN_RSS[0]), zeros(
(RNnum_RSS - 1, 1))))
C_RSS = RoAStd[1:RNnum_RSS, 0]**2
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA * RN_ToA, axis=0)).reshape(RNnum_ToA, 1)
RoA = c * ToA
RoAStd = c * ToAStd
RoA2 = (RoA * RoA).reshape(RNnum_ToA, 1)
K_ToA = RN_ToA2[1:RNnum_ToA, :] - RN_ToA2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_ToA, :]
A_ToA = hstack(
(RN_ToA[:, 1:RNnum_ToA].T -
RN_ToA[:, 0].reshape(1, shRN_ToA[0]), zeros(
(RNnum_ToA - 1, 1))))
C_ToA = RoAStd[1:RNnum_ToA, 0]**2
# solution
K = vstack((K_RSS, K_ToA))
A = vstack((A_RSS, A_ToA))
C = diag(hstack((C_RSS, C_ToA)))
A2 = dot(A.T, dot(linalg.inv(C), A))
[U, S, V] = svd(A2)
J = 1 / S
rA = rank(A)
m, n = shape(A)
f = 0
if log10(cond(A2)) >= max(
RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)):
f = f + 1
for i in range(n - rA):
u = where(J == max(J))
J[u] = 0
A2i = dot(dot(V.T, diag(J)), U.T)
P = 0.5 * dot(A2i, dot(dot(A.T, linalg.inv(C)), K))
return P[:2, :]
else:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS * RN_RSS, axis=0)).reshape(RNnum_RSS, 1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)
RoA2 = (RoA * RoA).reshape(RNnum_RSS, 1)
K_RSS = RN_RSS2[1:RNnum_RSS, :] - RN_RSS2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_RSS, :]
A_RSS = hstack(
(RN_RSS[:, 1:RNnum_RSS].T -
RN_RSS[:, 0].reshape(1, shRN_RSS[0]), zeros(
(RNnum_RSS - 1, 1))))
C_RSS = RoAStd[1:RNnum_RSS, 0]**2
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA * RN_ToA, axis=0)).reshape(RNnum_ToA, 1)
RoA = c * ToA
RoAStd = c * ToAStd
RoA2 = (RoA * RoA).reshape(RNnum_ToA, 1)
K_ToA = RN_ToA2[1:RNnum_ToA, :] - RN_ToA2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_ToA, :]
A_ToA = hstack(
(RN_ToA[:, 1:RNnum_ToA].T -
RN_ToA[:, 0].reshape(1, shRN_ToA[0]), zeros(
(RNnum_ToA - 1, 1))))
C_ToA = RoAStd[1:RNnum_ToA, 0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_TDoA[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c * TDoA
RDoAStd = c * TDoAStd
RDoA2 = (RDoA * RDoA).reshape(RNnum_TDoA, 1)
K_TDoA = (sum(
(RN_TDoA - RN_TDoA2) *
(RN_TDoA - RN_TDoA2), axis=0)).reshape(RNnum_TDoA, 1) - RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T, 0.5 * RoA[0, 0] * RDoA))
C_TDoA = RDoAStd[:, 0]**2
# solution
K = vstack((vstack((K_RSS, K_ToA)), K_TDoA))
A = vstack((vstack((A_RSS, A_ToA)), A_TDoA))
C = diag(hstack((hstack((C_RSS, C_ToA)), C_TDoA)))
A2 = dot(A.T, dot(linalg.inv(C), A))
[U, S, V] = svd(A2)
J = 1 / S
rA = rank(A)
m, n = shape(A)
f = 0
if log10(cond(A2)) >= max(
RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)):
f = f + 1
for i in range(n - rA):
u = where(J == max(J))
J[u] = 0
A2i = dot(dot(V.T, diag(J)), U.T)
P = 0.5 * dot(A2i, dot(dot(A.T, linalg.inv(C)), K))
return P[:2, :]
def WLSHDFLocate(self, RN_RSS, RN_ToA, RN_TDoA, RN_TDoA2, ToA, ToAStd,
TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest):
"""
This applies LS approximation to get position P.
Return P
"""
c = 3e08
RSSL = RSSLocation(RN_RSS)
if RN_RSS == None:
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA * RN_ToA, axis=0)).reshape(RNnum_ToA, 1)
RoA = c * ToA
RoAStd = c * ToAStd
RoA2 = (RoA * RoA).reshape(RNnum_ToA, 1)
K_ToA = RN_ToA2[1:RNnum_ToA, :] - RN_ToA2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_ToA, :]
A_ToA = hstack(
(RN_ToA[:, 1:RNnum_ToA].T -
RN_ToA[:, 0].reshape(1, shRN_ToA[0]), zeros(
(RNnum_ToA - 1, 1))))
C_ToA = RoAStd[1:RNnum_ToA, 0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_ToA[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c * TDoA
RDoAStd = c * TDoAStd
RDoA2 = (RDoA * RDoA).reshape(RNnum_TDoA, 1)
K_TDoA = (sum(
(RN_TDoA - RN_TDoA2) *
(RN_TDoA - RN_TDoA2), axis=0)).reshape(RNnum_TDoA, 1) - RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T, 0.5 * RoA[0, 0] * RDoA))
C_TDoA = RDoAStd[:, 0]**2
# solution
K = vstack((K_ToA, K_TDoA))
A = vstack((A_ToA, A_TDoA))
C = diag(hstack((C_ToA, C_TDoA)))
P = 0.5 * dot(linalg.inv(dot(A.T, dot(linalg.inv(C), A))),
dot(dot(A.T, linalg.inv(C)), K))
return P[:2, :]
elif RN_ToA == None:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS * RN_RSS, axis=0)).reshape(RNnum_RSS, 1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)
RoA2 = (RoA * RoA).reshape(RNnum_RSS, 1)
K_RSS = RN_RSS2[1:RNnum_RSS, :] - RN_RSS2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_RSS, :]
A_RSS = hstack(
(RN_RSS[:, 1:RNnum_RSS].T -
RN_RSS[:, 0].reshape(1, shRN_RSS[0]), zeros(
(RNnum_RSS - 1, 1))))
C_RSS = RoAStd[1:RNnum_RSS, 0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_RSS[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c * TDoA
RDoAStd = c * TDoAStd
RDoA2 = (RDoA * RDoA).reshape(RNnum_TDoA, 1)
K_TDoA = (sum(
(RN_TDoA - RN_TDoA2) *
(RN_TDoA - RN_TDoA2), axis=0)).reshape(RNnum_TDoA, 1) - RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T, 0.5 * RoA[0, 0] * RDoA))
C_TDoA = RDoAStd[:, 0]**2
# solution
K = vstack((K_RSS, K_TDoA))
A = vstack((A_RSS, A_TDoA))
C = diag(hstack((C_RSS, C_TDoA)))
P = 0.5 * dot(linalg.inv(dot(A.T, dot(linalg.inv(C), A))),
dot(dot(A.T, linalg.inv(C)), K))
return P[:2, :]
elif RN_TDoA == None:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS * RN_RSS, axis=0)).reshape(RNnum_RSS, 1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)
RoA2 = (RoA * RoA).reshape(RNnum_RSS, 1)
K_RSS = RN_RSS2[1:RNnum_RSS, :] - RN_RSS2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_RSS, :]
A_RSS = hstack(
(RN_RSS[:, 1:RNnum_RSS].T -
RN_RSS[:, 0].reshape(1, shRN_RSS[0]), zeros(
(RNnum_RSS - 1, 1))))
C_RSS = RoAStd[1:RNnum_RSS, 0]**2
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA * RN_ToA, axis=0)).reshape(RNnum_ToA, 1)
RoA = c * ToA
RoAStd = c * ToAStd
RoA2 = (RoA * RoA).reshape(RNnum_ToA, 1)
K_ToA = RN_ToA2[1:RNnum_ToA, :] - RN_ToA2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_ToA, :]
A_ToA = hstack(
(RN_ToA[:, 1:RNnum_ToA].T -
RN_ToA[:, 0].reshape(1, shRN_ToA[0]), zeros(
(RNnum_ToA - 1, 1))))
C_ToA = RoAStd[1:RNnum_ToA, 0]**2
# solution
K = vstack((K_RSS, K_ToA))
A = vstack((A_RSS, A_ToA))
C = diag(hstack((C_RSS, C_ToA)))
P = 0.5 * dot(linalg.inv(dot(A.T, dot(linalg.inv(C), A))),
dot(dot(A.T, linalg.inv(C)), K))
return P
else:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS * RN_RSS, axis=0)).reshape(RNnum_RSS, 1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)
RoA2 = (RoA * RoA).reshape(RNnum_RSS, 1)
K_RSS = RN_RSS2[1:RNnum_RSS, :] - RN_RSS2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_RSS, :]
A_RSS = hstack(
(RN_RSS[:, 1:RNnum_RSS].T -
RN_RSS[:, 0].reshape(1, shRN_RSS[0]), zeros(
(RNnum_RSS - 1, 1))))
C_RSS = RoAStd[1:RNnum_RSS, 0]**2
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA * RN_ToA, axis=0)).reshape(RNnum_ToA, 1)
RoA = c * ToA
RoAStd = c * ToAStd
RoA2 = (RoA * RoA).reshape(RNnum_ToA, 1)
K_ToA = RN_ToA2[1:RNnum_ToA, :] - RN_ToA2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_ToA, :]
A_ToA = hstack(
(RN_ToA[:, 1:RNnum_ToA].T -
RN_ToA[:, 0].reshape(1, shRN_ToA[0]), zeros(
(RNnum_ToA - 1, 1))))
C_ToA = RoAStd[1:RNnum_ToA, 0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_TDoA[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c * TDoA
RDoAStd = c * TDoAStd
RDoA2 = (RDoA * RDoA).reshape(RNnum_TDoA, 1)
K_TDoA = (sum(
(RN_TDoA - RN_TDoA2) *
(RN_TDoA - RN_TDoA2), axis=0)).reshape(RNnum_TDoA, 1) - RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T, 0.5 * RoA[0, 0] * RDoA))
C_TDoA = RDoAStd[:, 0]**2
# solution
K = vstack((vstack((K_RSS, K_ToA)), K_TDoA))
A = vstack((vstack((A_RSS, A_ToA)), A_TDoA))
C = diag(hstack((hstack((C_RSS, C_ToA)), C_TDoA)))
P = 0.5 * dot(linalg.inv(dot(A.T, dot(linalg.inv(C), A))),
dot(dot(A.T, linalg.inv(C)), K))
return P[:2, :]
def TWLSHDFLocate(self, RN_RSS, RN_ToA, RN_TDoA, RN_TDoA2, ToA, ToAStd, TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest):
"""
This applies LS approximation to get position P.
Return P
"""
c = 3e08
RSSL=RSSLocation(RN_RSS)
if RN_RSS==None:
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA*RN_ToA,axis=0)).reshape(RNnum_ToA,1)
RoA = c*ToA
RoAStd = c*ToAStd
RoA2 = (RoA*RoA).reshape(RNnum_ToA,1)
K_ToA = RN_ToA2[1:RNnum_ToA,:]-RN_ToA2[0,0] + RoA2[0,0]-RoA2[1:RNnum_ToA,:]
A_ToA = hstack((RN_ToA[:,1:RNnum_ToA].T - RN_ToA[:,0].reshape(1,shRN_ToA[0]), zeros((RNnum_ToA-1,1))))
C_ToA = RoAStd[1:RNnum_ToA,0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_ToA[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c*TDoA
RDoAStd = c*TDoAStd
RDoA2 = (RDoA*RDoA).reshape(RNnum_TDoA,1)
K_TDoA = (sum((RN_TDoA-RN_TDoA2)*(RN_TDoA-RN_TDoA2),axis=0)).reshape(RNnum_TDoA,1)-RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T,0.5*RoA[0,0]*RDoA))
C_TDoA = RDoAStd[:,0]**2
# solution
K=vstack((K_ToA, K_TDoA))
A=vstack((A_ToA, A_TDoA))
C=diag(hstack((C_ToA, C_TDoA)))
A2 = dot(A.T,dot(linalg.inv(C),A))
[U,S,V]=svd(A2)
J = 1/S
rA=rank(A)
m,n=shape(A)
f=0
if log10(cond(A2))>=max(RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)):
f=f+1
for i in range(n-rA):
u = where(J==max(J))
J[u] = 0
A2i = dot(dot(V.T,diag(J)),U.T)
P = 0.5*dot(A2i,dot(dot(A.T,linalg.inv(C)),K))
return P[:2,:]
elif RN_ToA==None:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS*RN_RSS,axis=0)).reshape(RNnum_RSS,1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)
RoA2 = (RoA*RoA).reshape(RNnum_RSS,1)
K_RSS = RN_RSS2[1:RNnum_RSS,:]-RN_RSS2[0,0] + RoA2[0,0]-RoA2[1:RNnum_RSS,:]
A_RSS = hstack((RN_RSS[:,1:RNnum_RSS].T - RN_RSS[:,0].reshape(1,shRN_RSS[0]), zeros((RNnum_RSS-1,1))))
C_RSS = RoAStd[1:RNnum_RSS,0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_RSS[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c*TDoA
RDoAStd = c*TDoAStd
RDoA2 = (RDoA*RDoA).reshape(RNnum_TDoA,1)
K_TDoA = (sum((RN_TDoA-RN_TDoA2)*(RN_TDoA-RN_TDoA2),axis=0)).reshape(RNnum_TDoA,1)-RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T,0.5*RoA[0,0]*RDoA))
C_TDoA = RDoAStd[:,0]**2
# solution
K=vstack((K_RSS, K_TDoA))
A=vstack((A_RSS, A_TDoA))
C=diag(hstack((C_RSS, C_TDoA)))
A2 = dot(A.T,dot(linalg.inv(C),A))
[U,S,V]=svd(A2)
J = 1/S
rA=rank(A)
m,n=shape(A)
f=0
if log10(cond(A2))>=max(RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)):
f=f+1
for i in range(n-rA):
u = where(J==max(J))
J[u] = 0
A2i = dot(dot(V.T,diag(J)),U.T)
P = 0.5*dot(A2i,dot(dot(A.T,linalg.inv(C)),K))
return P[:2,:]
elif RN_TDoA==None:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS*RN_RSS,axis=0)).reshape(RNnum_RSS,1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)
RoA2 = (RoA*RoA).reshape(RNnum_RSS,1)
K_RSS = RN_RSS2[1:RNnum_RSS,:]-RN_RSS2[0,0] + RoA2[0,0]-RoA2[1:RNnum_RSS,:]
A_RSS = hstack((RN_RSS[:,1:RNnum_RSS].T - RN_RSS[:,0].reshape(1,shRN_RSS[0]), zeros((RNnum_RSS-1,1))))
C_RSS = RoAStd[1:RNnum_RSS,0]**2
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA*RN_ToA,axis=0)).reshape(RNnum_ToA,1)
RoA = c*ToA
RoAStd = c*ToAStd
RoA2 = (RoA*RoA).reshape(RNnum_ToA,1)
K_ToA = RN_ToA2[1:RNnum_ToA,:]-RN_ToA2[0,0] + RoA2[0,0]-RoA2[1:RNnum_ToA,:]
A_ToA = hstack((RN_ToA[:,1:RNnum_ToA].T - RN_ToA[:,0].reshape(1,shRN_ToA[0]), zeros((RNnum_ToA-1,1))))
C_ToA = RoAStd[1:RNnum_ToA,0]**2
# solution
K=vstack((K_RSS, K_ToA))
A=vstack((A_RSS, A_ToA))
C=diag(hstack((C_RSS, C_ToA)))
A2 = dot(A.T,dot(linalg.inv(C),A))
[U,S,V]=svd(A2)
J = 1/S
rA=rank(A)
m,n=shape(A)
f=0
if log10(cond(A2))>=max(RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)):
f=f+1
for i in range(n-rA):
u = where(J==max(J))
J[u] = 0
A2i = dot(dot(V.T,diag(J)),U.T)
P = 0.5*dot(A2i,dot(dot(A.T,linalg.inv(C)),K))
return P[:2,:]
else:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS*RN_RSS,axis=0)).reshape(RNnum_RSS,1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)
RoA2 = (RoA*RoA).reshape(RNnum_RSS,1)
K_RSS = RN_RSS2[1:RNnum_RSS,:]-RN_RSS2[0,0] + RoA2[0,0]-RoA2[1:RNnum_RSS,:]
A_RSS = hstack((RN_RSS[:,1:RNnum_RSS].T - RN_RSS[:,0].reshape(1,shRN_RSS[0]), zeros((RNnum_RSS-1,1))))
C_RSS = RoAStd[1:RNnum_RSS,0]**2
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA*RN_ToA,axis=0)).reshape(RNnum_ToA,1)
RoA = c*ToA
RoAStd = c*ToAStd
RoA2 = (RoA*RoA).reshape(RNnum_ToA,1)
K_ToA = RN_ToA2[1:RNnum_ToA,:]-RN_ToA2[0,0] + RoA2[0,0]-RoA2[1:RNnum_ToA,:]
A_ToA = hstack((RN_ToA[:,1:RNnum_ToA].T - RN_ToA[:,0].reshape(1,shRN_ToA[0]), zeros((RNnum_ToA-1,1))))
C_ToA = RoAStd[1:RNnum_ToA,0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_TDoA[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c*TDoA
RDoAStd = c*TDoAStd
RDoA2 = (RDoA*RDoA).reshape(RNnum_TDoA,1)
K_TDoA = (sum((RN_TDoA-RN_TDoA2)*(RN_TDoA-RN_TDoA2),axis=0)).reshape(RNnum_TDoA,1)-RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T,0.5*RoA[0,0]*RDoA))
C_TDoA = RDoAStd[:,0]**2
# solution
K=vstack((vstack((K_RSS,K_ToA)), K_TDoA))
A=vstack((vstack((A_RSS,A_ToA)), A_TDoA))
C=diag(hstack((hstack((C_RSS,C_ToA)), C_TDoA)))
A2 = dot(A.T,dot(linalg.inv(C),A))
[U,S,V]=svd(A2)
J = 1/S
rA=rank(A)
m,n=shape(A)
f=0
if log10(cond(A2))>=max(RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)):
f=f+1
for i in range(n-rA):
u = where(J==max(J))
J[u] = 0
A2i = dot(dot(V.T,diag(J)),U.T)
P = 0.5*dot(A2i,dot(dot(A.T,linalg.inv(C)),K))
return P[:2,:]
class RSS(Constraint):
""" RSS Constraint
Description and evaluation of TOA constraints
:Parameters:
value : float
Constraint value in dB. Default = 40
std : float
Value standard deviation in dB. default = 1.0
vcw : float
scale factor. Default = 1.0
model : dictionnary
keys: self.model['PL0'] =-34.7
self.model['d0'] = 1.0
self.model['RSSnp'] = 2.64
self.model['RSSStd'] = 4.34
self.model['Rest'] = 'mode'
p : np.array 1 x ndim
constraint center
:Attributes:
range : distance conversion from time self.value.
sstd : distance conversion from time self.std
runable : True NOT USED
evaluated :False NOT USED
self.Id : Constraint ID
from annulus bound:
min : minimum value of observable
max : maximum value of observable
mean : mean value of observable
:Methods:
annulus_bound(self) : Compute the minimum and maximum distance of the enclosing annulus of the constraint
rescale(self,vcw) : rescale contraint boundary with a given scale factor 'vcw'
inclusive(self,b) : Is constraint center is inside a box ?
valid(self,b) : Test if Lbox is compatible with the constraint
valid_v(self,lv) : Test if a liste of a vertexes from a box is compatible with the constraint. vertexes are obtained thanks to LBoxN.bd2coordinates()
estvol(self) : Constraint Volume estimation
"""
def __init__(self,value=40,std=1.0,vcw=3,model={},p=np.array([])):
Constraint.__init__(self,'RSS',p)
self.Id = copy.copy(self.C_Id)
Constraint.C_Id = Constraint.C_Id+1 # constraint counter is incremented
self.value = value # attennation (dB)
if len(model)==0:
self.model['PL0'] =-34.7
self.model['d0'] = 1.0
self.model['RSSnp'] = 2.64
self.model['RSSStd'] = 4.34
self.model['Rest'] = 'mode'
else :
self.model = model
self.LOC = RSSLocation(p)
self.std = (self.LOC.getRangeStd(p, self.model['PL0'], self.model['d0'], self.value,self.model['RSSnp'], self.model['RSSStd'], self.model['Rest'])[0])/0.3
self.range = self.LOC.getRange(p, self.model['PL0'], self.model['d0'], self.value, self.model['RSSnp'], self.model['RSSStd'], self.model['Rest'])[0]
self.sstd = self.std*0.3
self.rescale(vcw)
self.runable = True
self.evaluated = False
self.annulus_bound()
def annulus_bound(self):
"""
annulus_bound():
Compute the minimum and maximum distance of the enclosing annulus of the constraint
:Returns:
Nothing but update cmin, cmax
"""
self.cmin = max(0,self.range - self.vcw*self.sstd )
self.cmax = self.range + self.vcw*self.sstd
def rescale(self,vcw):
"""rescale bounds of the constraint with vcw factor
:Parameters:
vcw : float
scale factor
"""
self.vcw = vcw
dx = self.range+(self.vcw*self.std)*0.3*np.ones(len(self.p))
box = BoxN(np.array([self.p-dx,self.p+dx]),ndim=len(self.p))
self.lbox = LBoxN([box])
self.estvol()
def inclusive(self,b):
""" Is constraint center is inside a box ?
:Parameters:
b : LBoxN
:Returns:
Boolean
"""
if b.inbox(self.p):
return True
else:
return False
def valid(self,b):
"""check if a box is valid for the given constraint
:Parameters:
b : LBoxN
:Return:
True : if box is enclosed
False : if the box is ambiguous
'Out' : if the box is out
"""
p0 = self.p
v = self.bd2coord(b)
P = np.outer(np.ones(len(v)),p0)
D = P-v
DD = np.sqrt(np.sum(D*D,axis=1))
DDcmin = sum(DD>=cmin)
DDcmax = sum(DD<=cmax)
if DDcmin+DDcmax > 15:
return(True)
elif (DDcmin <1) | (DDcmax <1) :#(bmax<cmin)|(bmin>cmax):
return('out')
else :
return(False)
def valid_v(self,v):
"""
.. todo:
update as TOA valid_v !!
valid_v(v) : check if vertex are valid for the given constraint
A box is valid if it not not valid
A box is not valid if all distances are greater than rangemax
or all distances are less than rangemin
"""
p0 = self.p
P = np.outer(np.ones(len(v)),p0)
D = P-v
DD = np.sqrt(np.sum(D*D,axis=1))
DD2 = (DD>=self.cmin) & (DD<=self.cmax)
return DD2
def estvol(self):
""" Constraint Volume estimation
"""
R2 = self.range+self.vcw*self.sstd
R1 = self.range-self.vcw*self.sstd
V2 = (4.*np.pi*R2**3)/3
V1 = (4.*np.pi*R1**3)/3
self.estvlm = V2-V1
def WLSHDFLocate(self, RN_RSS, RN_ToA, RN_TDoA, RN_TDoA2, ToA, ToAStd, TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest):
"""
This applies LS approximation to get position P.
Return P
"""
c = 3e08
RSSL=RSSLocation(RN_RSS)
if RN_RSS==None:
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA*RN_ToA,axis=0)).reshape(RNnum_ToA,1)
RoA = c*ToA
RoAStd = c*ToAStd
RoA2 = (RoA*RoA).reshape(RNnum_ToA,1)
K_ToA = RN_ToA2[1:RNnum_ToA,:]-RN_ToA2[0,0] + RoA2[0,0]-RoA2[1:RNnum_ToA,:]
A_ToA = hstack((RN_ToA[:,1:RNnum_ToA].T - RN_ToA[:,0].reshape(1,shRN_ToA[0]), zeros((RNnum_ToA-1,1))))
C_ToA = RoAStd[1:RNnum_ToA,0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_ToA[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c*TDoA
RDoAStd = c*TDoAStd
RDoA2 = (RDoA*RDoA).reshape(RNnum_TDoA,1)
K_TDoA = (sum((RN_TDoA-RN_TDoA2)*(RN_TDoA-RN_TDoA2),axis=0)).reshape(RNnum_TDoA,1)-RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T,0.5*RoA[0,0]*RDoA))
C_TDoA = RDoAStd[:,0]**2
# solution
K=vstack((K_ToA, K_TDoA))
A=vstack((A_ToA, A_TDoA))
C=diag(hstack((C_ToA, C_TDoA)))
P = 0.5*dot(linalg.inv(dot(A.T,dot(linalg.inv(C),A))),dot(dot(A.T,linalg.inv(C)),K))
return P[:2,:]
elif RN_ToA==None:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS*RN_RSS,axis=0)).reshape(RNnum_RSS,1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)
RoA2 = (RoA*RoA).reshape(RNnum_RSS,1)
K_RSS = RN_RSS2[1:RNnum_RSS,:]-RN_RSS2[0,0] + RoA2[0,0]-RoA2[1:RNnum_RSS,:]
A_RSS = hstack((RN_RSS[:,1:RNnum_RSS].T - RN_RSS[:,0].reshape(1,shRN_RSS[0]), zeros((RNnum_RSS-1,1))))
C_RSS = RoAStd[1:RNnum_RSS,0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_RSS[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c*TDoA
RDoAStd = c*TDoAStd
RDoA2 = (RDoA*RDoA).reshape(RNnum_TDoA,1)
K_TDoA = (sum((RN_TDoA-RN_TDoA2)*(RN_TDoA-RN_TDoA2),axis=0)).reshape(RNnum_TDoA,1)-RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T,0.5*RoA[0,0]*RDoA))
C_TDoA = RDoAStd[:,0]**2
# solution
K=vstack((K_RSS, K_TDoA))
A=vstack((A_RSS, A_TDoA))
C=diag(hstack((C_RSS, C_TDoA)))
P = 0.5*dot(linalg.inv(dot(A.T,dot(linalg.inv(C),A))),dot(dot(A.T,linalg.inv(C)),K))
return P[:2,:]
elif RN_TDoA==None:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS*RN_RSS,axis=0)).reshape(RNnum_RSS,1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)
RoA2 = (RoA*RoA).reshape(RNnum_RSS,1)
K_RSS = RN_RSS2[1:RNnum_RSS,:]-RN_RSS2[0,0] + RoA2[0,0]-RoA2[1:RNnum_RSS,:]
A_RSS = hstack((RN_RSS[:,1:RNnum_RSS].T - RN_RSS[:,0].reshape(1,shRN_RSS[0]), zeros((RNnum_RSS-1,1))))
C_RSS = RoAStd[1:RNnum_RSS,0]**2
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA*RN_ToA,axis=0)).reshape(RNnum_ToA,1)
RoA = c*ToA
RoAStd = c*ToAStd
RoA2 = (RoA*RoA).reshape(RNnum_ToA,1)
K_ToA = RN_ToA2[1:RNnum_ToA,:]-RN_ToA2[0,0] + RoA2[0,0]-RoA2[1:RNnum_ToA,:]
A_ToA = hstack((RN_ToA[:,1:RNnum_ToA].T - RN_ToA[:,0].reshape(1,shRN_ToA[0]), zeros((RNnum_ToA-1,1))))
C_ToA = RoAStd[1:RNnum_ToA,0]**2
# solution
K=vstack((K_RSS, K_ToA))
A=vstack((A_RSS, A_ToA))
C=diag(hstack((C_RSS, C_ToA)))
P = 0.5*dot(linalg.inv(dot(A.T,dot(linalg.inv(C),A))),dot(dot(A.T,linalg.inv(C)),K))
return P
else:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS*RN_RSS,axis=0)).reshape(RNnum_RSS,1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)
RoA2 = (RoA*RoA).reshape(RNnum_RSS,1)
K_RSS = RN_RSS2[1:RNnum_RSS,:]-RN_RSS2[0,0] + RoA2[0,0]-RoA2[1:RNnum_RSS,:]
A_RSS = hstack((RN_RSS[:,1:RNnum_RSS].T - RN_RSS[:,0].reshape(1,shRN_RSS[0]), zeros((RNnum_RSS-1,1))))
C_RSS = RoAStd[1:RNnum_RSS,0]**2
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA*RN_ToA,axis=0)).reshape(RNnum_ToA,1)
RoA = c*ToA
RoAStd = c*ToAStd
RoA2 = (RoA*RoA).reshape(RNnum_ToA,1)
K_ToA = RN_ToA2[1:RNnum_ToA,:]-RN_ToA2[0,0] + RoA2[0,0]-RoA2[1:RNnum_ToA,:]
A_ToA = hstack((RN_ToA[:,1:RNnum_ToA].T - RN_ToA[:,0].reshape(1,shRN_ToA[0]), zeros((RNnum_ToA-1,1))))
C_ToA = RoAStd[1:RNnum_ToA,0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_TDoA[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c*TDoA
RDoAStd = c*TDoAStd
RDoA2 = (RDoA*RDoA).reshape(RNnum_TDoA,1)
K_TDoA = (sum((RN_TDoA-RN_TDoA2)*(RN_TDoA-RN_TDoA2),axis=0)).reshape(RNnum_TDoA,1)-RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T,0.5*RoA[0,0]*RDoA))
C_TDoA = RDoAStd[:,0]**2
# solution
K=vstack((vstack((K_RSS,K_ToA)), K_TDoA))
A=vstack((vstack((A_RSS,A_ToA)), A_TDoA))
C=diag(hstack((hstack((C_RSS,C_ToA)), C_TDoA)))
P = 0.5*dot(linalg.inv(dot(A.T,dot(linalg.inv(C),A))),dot(dot(A.T,linalg.inv(C)),K))
return P[:2,:]
