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