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,:]