def get_rms(truePos, estimated, align=False, scale=False, norm=False):
"""
Returns root mean square error of estimated positions.
Set align if estimated positions need to be transformed (rotated and
translated) before calculating rms error of localization. Set scale=True
if they needs to be scaled also.
If norm is True then rms is divided with norm of the true formation
translated to centroid.
"""
truePos = Positions.create(truePos)
assert(len(truePos) == 1)
estimated = Positions.create(estimated)
if align:
estimated = deepcopy(estimated)
align_clusters(truePos, estimated, scale)
suma = 0
node_count = 0
for estimated_subcluster in estimated:
for n in estimated_subcluster:
suma += (estimated_subcluster[n][0] - truePos[0][n][0]) ** 2 + \
(estimated_subcluster[n][1] - truePos[0][n][1]) ** 2
node_count += 1
rms = sqrt(suma / node_count)
if norm:
sc = truePos.subclusters[0]
truePos.subclusters[0] = {n: p for n, p in sc.items()
if n in estimated.subclusters[0]}
rms = rms/get_pos_norm(truePos)
return rms
def show_localized(net, estimated, scale=False, align=True,\
display_loc_err=True, show_labels=True):
"""
Display estimated positions.
estimated should be a list of dictionaries.
"""
from matplotlib.pylab import gca
from matplotlib.collections import LineCollection
truePos = Positions.create(net.pos)
estimated = Positions.create(estimated)
# copy estimated so that passed estimated remains unchanged
estimated = deepcopy(estimated)
if align:
# rotate, translate and if needed scale estimated w.r.t. true positions
align_clusters(truePos, estimated, scale)
#TODO: implement display of all subclusters
if len(estimated) > 1:
raise (NotImplementedError)
else:
estimated_sc = estimated[0]
#net.show(positions=estimated_sc, show_labels=show_labels)
fig = net.get_fig(positions=estimated_sc, show_labels=show_labels)
ax = fig.gca()
minpos = min(estimated_sc.values(), axis=0)
maxpos = max(estimated_sc.values(), axis=0)
minpos -= (maxpos - minpos) * 0.1
maxpos += (maxpos - minpos) * 0.1
ax.set_xlim(minpos[0], maxpos[0])
ax.set_ylim(minpos[1], maxpos[1])
#fig.show()
if display_loc_err:
#TODO: not working in ipython notepad
ax = gca()
ax.set_title('Localized positions')
ax.set_title('Localization error display')
edge_pos = asarray([(net.pos[n], estimated_sc[n])
for n in estimated_sc.keys()])
errorCollection = LineCollection(edge_pos,
colors='r',
transOffset=ax.transData)
errorCollection.set_zorder(1) # errors go behind nodes
ax.add_collection(errorCollection)
ax.figure.canvas.draw()
def show_localized(net, estimated, scale=False, align=True,\
display_loc_err=True, show_labels=True):
"""
Display estimated positions.
estimated should be a list of dictionaries.
"""
from matplotlib.pylab import gca
from matplotlib.collections import LineCollection
truePos = Positions.create(net.pos)
estimated = Positions.create(estimated)
# copy estimated so that passed estimated remains unchanged
estimated = deepcopy(estimated)
if align:
# rotate, translate and if needed scale estimated w.r.t. true positions
align_clusters(truePos, estimated, scale)
#TODO: implement display of all subclusters
if len(estimated)>1:
raise(NotImplementedError)
else:
estimated_sc = estimated[0]
#net.show(positions=estimated_sc, show_labels=show_labels)
fig = net.get_fig(positions=estimated_sc, show_labels=show_labels)
ax = fig.gca()
minpos = min(estimated_sc.values(), axis=0)
maxpos = max(estimated_sc.values(), axis=0)
minpos -= (maxpos-minpos)*0.1
maxpos += (maxpos-minpos)*0.1
ax.set_xlim(minpos[0], maxpos[0])
ax.set_ylim(minpos[1], maxpos[1])
#fig.show()
if display_loc_err:
#TODO: not working in ipython notepad
ax = gca()
ax.set_title('Localized positions')
ax.set_title('Localization error display')
edge_pos = asarray([(net.pos[n], estimated_sc[n])
for n in estimated_sc.keys()])
errorCollection = LineCollection(edge_pos, colors='r',
transOffset=ax.transData)
errorCollection.set_zorder(1) # errors go behind nodes
ax.add_collection(errorCollection)
ax.figure.canvas.draw()
def on_actionShowLocalizedSubclusters_triggered(self):
if len(self.ui.nodeInspector.selectedIndexes()) == 1:
qModelIndex = self.ui.nodeInspector.selectedIndexes()[0]
treeItem = qModelIndex.internalPointer()
assert(isinstance(treeItem.itemDataValue, Positions))
estimated = deepcopy(treeItem.itemDataValue)
estimatedsub = estimated.subclusters[0]
# rotate, translate and optionally scale
# w.r.t. original positions (pos)
align_clusters(Positions.create(self.net.pos), estimated, True)
net = self.net.subnetwork(estimatedsub.keys(), pos=estimatedsub)
self.draw_network(net=net, drawMessages=False)
edge_pos = numpy.asarray([(self.net.pos[node], estimatedsub[node][:2])
for node in net])
error_collection = LineCollection(edge_pos, colors='r')
self.axes.add_collection(error_collection)
rms = get_rms(self.net.pos, estimated, scale=False)
self.update_log('rms = %.3f' % rms)
self.update_log('localized = %.2f%% (%d/%d)' %
(len(estimatedsub) * 1. / len(self.net.pos) * 100,
len(estimatedsub), len(self.net.pos)))
def on_actionShowLocalizedSubclusters_triggered(self):
if len(self.ui.nodeInspector.selectedIndexes()) == 1:
qModelIndex = self.ui.nodeInspector.selectedIndexes()[0]
treeItem = qModelIndex.internalPointer()
assert (isinstance(treeItem.itemDataValue, Positions))
estimated = deepcopy(treeItem.itemDataValue)
estimatedsub = estimated.subclusters[0]
# rotate, translate and optionally scale
# w.r.t. original positions (pos)
align_clusters(Positions.create(self.net.pos), estimated, True)
net = self.net.subnetwork(estimatedsub.keys(), pos=estimatedsub)
self.draw_network(net=net, drawMessages=False)
edge_pos = numpy.asarray([
(self.net.pos[node], estimatedsub[node][:2]) for node in net
])
error_collection = LineCollection(edge_pos, colors='r')
self.axes.add_collection(error_collection)
rms = get_rms(self.net.pos, estimated, scale=False)
self.update_log('rms = %.3f' % rms)
self.update_log('localized = %.2f%% (%d/%d)' %
(len(estimatedsub) * 1. / len(self.net.pos) * 100,
len(estimatedsub), len(self.net.pos)))
def get_aoa_gdop_rel(estimated):
"""
Calculation of relative GDOP is based on CRB.
GDOP_rel = sigma_CRB/sigma_d = sqrt(tr((G^TG)^-1)/(sum(Di^2))/M)
As regular GDOP, it is not dependent on scale nor on sigma_AoA.
"""
estimated = Positions.create(estimated)
assert len(estimated.subclusters) == 1
pos = estimated.subclusters[0]
nodes = pos.keys()
edges = [
e for e in pos.keys()[0].network.edges()
if e[0] in nodes and e[1] in nodes
]
G = construct_G(pos, edges, 3, 'AoASensor')
J = dot(G.T, G)
cov = pinv(J)
di = diag(cov)
di = concatenate((di[::3], di[1::3]))
var_p = sum(di) / len(nodes)
distances = [
sqrt(dot(pos[n1][:2] - pos[n2][:2], pos[n1][:2] - pos[n2][:2]))
for n1, n2 in edges
]
var_d = sum(square(distances)) / len(edges)
return sqrt(var_p / var_d)
def setup_positions(self, subclusters):
""" Return new cluster with nodes form main_cluster and with
subclusters defined as in subcluster argument. """
positions = [{k: v
for k, v in self.test_cluster.items() if k in sub}
for sub in subclusters]
return Positions(positions)
def get_pos_norm(pos):
""" Translate positions so that centroid is in the origin and return mean
norm of the translated positions. """
pos = Positions.create(pos)
assert(len(pos) == 1)
n = len(pos[0])
p = zeros((n, 2))
for i, node in enumerate(pos[0]):
p[i, :] = pos[0][node]
centroid = p.sum(axis=0)/n
p -= tile(centroid, (n, 1))
p_norm = nsum(sqrt(nsum(square(p), axis=1)))/n
return p_norm
def get_pos_norm(pos):
""" Translate positions so that centroid is in the origin and return mean
norm of the translated positions. """
pos = Positions.create(pos)
assert (len(pos) == 1)
n = len(pos[0])
p = zeros((n, 2))
for i, node in enumerate(pos[0]):
p[i, :] = pos[0][node][:2]
centroid = p.sum(axis=0) / n
p -= tile(centroid, (n, 1))
p_norm = nsum(sqrt(nsum(square(p), axis=1))) / n
return p_norm
def get_rms(truePos, estimated, align=False, scale=False, norm=False):
"""
Returns root mean square error of estimated positions.
Set align if estimated positions need to be transformed (rotated and
translated) before calculating rms error of localization. Set scale=True
if they needs to be scaled also.
If norm is True then rms is divided with norm of the true formation
translated to centroid.
"""
truePos = Positions.create(truePos)
assert (len(truePos) == 1)
estimated = Positions.create(estimated)
if align:
estimated = deepcopy(estimated)
align_clusters(truePos, estimated, scale)
suma = 0
node_count = 0
for estimated_subcluster in estimated:
for n in estimated_subcluster:
suma += (estimated_subcluster[n][0] - truePos[0][n][0]) ** 2 + \
(estimated_subcluster[n][1] - truePos[0][n][1]) ** 2
node_count += 1
rms = sqrt(suma / node_count)
if norm:
sc = truePos.subclusters[0]
truePos.subclusters[0] = {
n: p
for n, p in sc.items() if n in estimated.subclusters[0]
}
rms = rms / get_pos_norm(truePos)
return rms
def get_aoa_gdop_rel(estimated):
"""
Calculation of relative GDOP is based on CRB.
GDOP_rel = sigma_CRB/sigma_d = sqrt(tr((G^TG)^-1)/(sum(Di^2))/M)
As regular GDOP, it is not dependent on scale nor on sigma_AoA.
"""
estimated = Positions.create(estimated)
assert len(estimated.subclusters)==1
pos = estimated.subclusters[0]
nodes = pos.keys()
edges = [e for e in pos.keys()[0].network.edges()
if e[0] in nodes and e[1] in nodes]
G = construct_G(pos, edges, 3, 'AoASensor')
J = dot(G.T, G)
cov = pinv(J)
di = diag(cov)
di = concatenate((di[::3], di[1::3]))
var_p = sum(di)/len(nodes)
distances = [sqrt(dot(pos[n1][:2] - pos[n2][:2],
pos[n1][:2] - pos[n2][:2]))
for n1, n2 in edges]
var_d = sum(square(distances))/len(edges)
return sqrt(var_p/var_d)
def get_aoa_gdops(estimated):
estimated = Positions.create(estimated)
assert len(estimated.subclusters)==1
estimated = estimated.subclusters[0]
return [get_aoa_gdop_node(estimated, node) for node in estimated]
def get_aoa_gdops(estimated):
estimated = Positions.create(estimated)
assert len(estimated.subclusters) == 1
estimated = estimated.subclusters[0]
return [get_aoa_gdop_node(estimated, node) for node in estimated]
