def _local_error(self, targetM, i):
pull_error = 0.
ivectors = self._x[:, i, :][self._neighborpairs[:, 0]]
jvectors = self._x[:, i, :][self._neighborpairs[:, 1]]
diffv = ivectors - jvectors
pull_error = linalg.trace(diffv.dot(targetM).dot(diffv.T))
push_error = 0.0
ivectors = self._x[:, i, :][self._set[:, 0]]
jvectors = self._x[:, i, :][self._set[:, 1]]
lvectors = self._x[:, i, :][self._set[:, 2]]
diffij = ivectors - jvectors
diffil = ivectors - lvectors
lossij = diffij.dot(targetM).dot(diffij.T)
lossil = diffil.dot(targetM).dot(diffil.T)
mask = T.neq(self._y[self._set[:, 0]], self._y[self._set[:, 2]])
push_error = linalg.trace(mask*T.maximum(lossij - lossil + 1, 0))
self.zerocount = T.eq(linalg.diag(mask*T.maximum(lossij - lossil + 1, 0)), 0).sum()
# print np.sqrt((i+1.0)/self.M)
# pull_error = pull_error * np.sqrt((i+1.0)/self.M)
# push_error = push_error * np.sqrt((i+1.0)/self.M)
return pull_error, push_error
def WLSRSSLocate(self, RN, PL0, d0, RSS, RSSnp, RSSStd, Rest):
""" applies WLS approximation on RSS assuming RSSStd to get position P.
Returns
-------
P : estimated position
"""
shRN = np.shape(RN) # shape of RN
RNnum = shRN[1] # Number of reference nodes
# Construct the vector K (see theory)
RN2 = (np.sum(RN*RN,axis=0)).reshape(RNnum,1)
k1 = RN2[1:RNnum,:]-RN2[0,0] # first half of K
RoA = self.getRange(RN, PL0, d0, RSS, RSSnp, RSSStd, Rest) # RSS based Ranges (meters)
RoAStd = self.getRangeStd(RN, PL0, d0, RSS, RSSnp, RSSStd, Rest)
RoA2 = (RoA*RoA).reshape(RNnum,1)
k2 = RoA2[0,0]-RoA2[1:RNnum,:] # second half of K
K = k1+k2
# Construct the matrix A (see theory)
A = RN[:,1:RNnum].T - RN[:,0].reshape(1,shRN[0])
# Construct the Covariance Matrix
C = la.diag((RoAStd[1:RNnum,0])**2)
# Apply LS operator
P = 0.5*np.dot(la.inv(np.dot(A.T,np.dot(la.inv(C),A))),
np.dot(np.dot(A.T,la.inv(C)),K))
# Return the estimated position
return P
def TWLSRSSLocate(self, RN, PL0, d0, RSS, RSSnp, RSSStd, Rest):
"""
This applies WLS approximation on RSS assuming RSSStd to get position P.
Return P
"""
shRN = np.shape(RN) # shape of RN
RNnum = shRN[1] # Number of reference nodes
# Construct the vector K (see theory)
RN2 = (np.sum(RN * RN, axis=0)).reshape(RNnum, 1)
k1 = RN2[1:RNnum, :] - RN2[0, 0] # first half of K
RoA = self.getRange(RN, PL0, d0, RSS, RSSnp, RSSStd,
Rest) # RSS based Ranges (meters)
RoAStd = self.getRangeStd(RN, PL0, d0, RSS, RSSnp, RSSStd, Rest)
RoA2 = (RoA * RoA).reshape(RNnum, 1)
k2 = RoA2[0, 0] - RoA2[1:RNnum, :] # second half of K
K = k1 + k2
# Construct the matrix A (see theory)
A = RN[:, 1:RNnum].T - RN[:, 0].reshape(1, shRN[0])
# Construct the Covariance Matrix
C = la.diag((RoAStd[1:RNnum, 0])**2)
A2 = np.dot(A.T, np.dot(la.inv(C), A))
[U, S, V] = la.svd(A2)
J = 1 / S
rA = la.rank(A)
m, n = np.shape(A)
f = 0
if np.log10(cond(A2)) >= max(
self.getRangeStd(RN, PL0, d0, RSS, RSSnp, RSSStd, Rest)):
f = f + 1
for i in range(n - rA):
u = np.where(J == max(J))
J[u] = 0
A2i = np.dot(np.dot(V.T, la.diag(J)), U.T)
P = 0.5 * np.dot(A2i, np.dot(np.dot(A.T, la.inv(C)), K))
return P
def TWLSRSSLocate(self, RN, PL0, d0, RSS, RSSnp, RSSStd, Rest):
"""
This applies WLS approximation on RSS assuming RSSStd to get position P.
Return P
"""
shRN = np.shape(RN) # shape of RN
RNnum = shRN[1] # Number of reference nodes
# Construct the vector K (see theory)
RN2 = (np.sum(RN*RN,axis=0)).reshape(RNnum,1)
k1 = RN2[1:RNnum,:]-RN2[0,0] # first half of K
RoA = self.getRange(RN, PL0, d0, RSS, RSSnp, RSSStd, Rest) # RSS based Ranges (meters)
RoAStd = self.getRangeStd(RN, PL0, d0, RSS, RSSnp, RSSStd, Rest)
RoA2 = (RoA*RoA).reshape(RNnum,1)
k2 = RoA2[0,0]-RoA2[1:RNnum,:] # second half of K
K = k1+k2
# Construct the matrix A (see theory)
A = RN[:,1:RNnum].T - RN[:,0].reshape(1,shRN[0])
# Construct the Covariance Matrix
C = la.diag((RoAStd[1:RNnum,0])**2)
A2 = np.dot(A.T,np.dot(la.inv(C),A))
[U,S,V] = la.svd(A2)
J = 1/S
rA = la.rank(A)
m,n = np.shape(A)
f=0
if np.log10(cond(A2))>=max(self.getRangeStd(RN, PL0, d0, RSS, RSSnp, RSSStd, Rest)):
f=f+1
for i in range(n-rA):
u = np.where(J==max(J))
J[u] = 0
A2i = np.dot(np.dot(V.T,la.diag(J)),U.T)
P = 0.5*np.dot(A2i,np.dot(np.dot(A.T,la.inv(C)),K))
return P
