Algoritmo en Python
# Localización por trilateración
# Ubicación de puntos en el mapa
# Girni 2020-10-07 propuesta: edelros@espol.edu.ec
import numpy as np
import sympy as sym
import matplotlib.pyplot as plt
import pandas as pd
import json
import girni_lora_libreria as girni
# INGRESO
# revisar parametros al inicio
modo = 'rx'
medida = 'rssi'
# archivos de entrada
arch_medUbAtrib = 'rsmP06_'+medida+'UbicaUsar01.txt'
arch_ecuaciones = 'rsmP06_ecuaciones01.json'
# archivos de salida
arch_trilatera = 'rsmP07_'+medida+'trilatera01.txt'
# grupo a procesar
mostrargrp = ['FIEC'] #['FIEC','FCNM','RECT','CIRC']
mostrartip = ['punto']
mostrarfuera = [0,1] #[0,1]
tolera_error = 2 # decimales en error
# Referencias
baliza = {'d1':'gtwRECT',
'd2':'gtwFIEC',
'd3':'gtwFCNM'}
# PROCEDIMIENTO
baliza_key = list(baliza.keys())
baliza_val = list(baliza.values())
# leer datos
with open(arch_ecuaciones) as json_file:
ecuacion = json.load(json_file)
tabla = pd.read_csv(arch_medUbAtrib, index_col='etiqueta')
tabla = pd.DataFrame(tabla)
# coordenadas de baliza
coordbaliza = {}
for cadabaliza in baliza:
cualbaliza = baliza[cadabaliza]
coordbaliza[cadabaliza] = [tabla['c_este'][cualbaliza]]
coordbaliza[cadabaliza].append(tabla['c_norte'][cualbaliza])
# inicializa ubicados, encontrados, intervalos fuera, errores
# ubicados tiene [distancia, distancia+1s, distancia +2s,total]
ubicados = np.zeros(shape=(3,4),dtype=int)
tabla['interv_fuera'] = ''
tabla['encontrado'] = np.nan
# suma de errores
sumatrilat = np.zeros(shape=(2,4),dtype=float)
sumagps = np.zeros(shape=(2,4),dtype=float)
sumagpsdx = np.zeros(shape=(2,4),dtype=float)
sumagpsdy = np.zeros(shape=(2,4),dtype=float)
# distancias y errores a cada baliza
for unabaliza in baliza_val:
donde = baliza_val.index(unabaliza)
cualbaliza = baliza_key[donde]
columna = medida+'_'+modo+'_'+cualbaliza
tabla['dist0_'+cualbaliza] = np.nan
tabla['dist1_'+cualbaliza] = np.nan
tabla['dist2_'+cualbaliza] = np.nan
# distancia estimada por punto
for cadapunto in tabla.index:
p_rssi = tabla[columna][cadapunto]
esgrupo = tabla['grupo'][cadapunto]
estipo = tabla['tipo'][cadapunto]
# ecuacion a usar
ecuacion_rssi = ecuacion[unabaliza]
if not(np.isnan(p_rssi)):
if (esgrupo in mostrargrp) and (estipo in mostrartip):
ubicados[2,donde] = ubicados[2,donde] + 1
dist = girni.dist_rssi(p_rssi,ecuacion_rssi)
[distancia,e_mean,e_1std,e_2std,interv_fuera] = dist
dist1 = distancia + e_1std
dist2 = distancia + e_2std
tabla.loc[cadapunto,'dist0_'+cualbaliza] = distancia
tabla.loc[cadapunto,'dist1_'+cualbaliza] = dist1
tabla.loc[cadapunto,'dist2_'+cualbaliza] = dist2
if interv_fuera>0:
if len(tabla['interv_fuera'][cadapunto])==0:
separador=''
else:
separador=','
tabla.loc[cadapunto,'interv_fuera'] = tabla['interv_fuera'][cadapunto]+separador+cualbaliza
procesados = np.max(ubicados[2,:])
# trilateración. estima localización cada punto
# inicializa cada punto a i*std
for i in range(0,3,1):
tabla['trilat_este_'+str(i)] = np.nan
tabla['trilat_norte_'+str(i)] = np.nan
tabla['trilat_error_'+str(i)] = np.nan
tabla['ubicado_'+str(i)] = np.nan
tabla['trilat_gps_error_'+str(i)] = np.nan
tabla['trilat_gps_dx_'+str(i)] = np.nan
tabla['trilat_gps_dy_'+str(i)] = np.nan
# procesa cada punto
for cadapunto in tabla.index:
esgrupo = tabla['grupo'][cadapunto]
if esgrupo in mostrargrp:
# coordenadas GPS del punto
c_este = tabla['c_este'][cadapunto]
c_norte = tabla['c_norte'][cadapunto]
# cada punto a i*std
punto = {}
for i in range(0,3,1):
punto[i] = {}
cuenta = 0
for unabaliza in baliza:
punto[i][unabaliza] = tabla['dist'+str(i)+'_'+unabaliza][cadapunto]
# si existe distancia
if not(np.isnan(punto[i][unabaliza])):
cuenta = cuenta + 1
# Hay 3 mediciones a baliza
if cuenta==3:
localiza = girni.trilatera(punto[i],
coordbaliza,
tolera = 10e-4)
baricentro = localiza['baricentro']
barerror = localiza['barerror']
poligono = localiza['poligono']
sumabaricentro = np.sum(baricentro)
# Hay coordenadas de baricentro
if not(np.isnan(sumabaricentro)):
tabla.loc[cadapunto,'trilat_este_'+str(i)] = baricentro[0]
tabla.loc[cadapunto,'trilat_norte_'+str(i)] = baricentro[1]
tabla.loc[cadapunto,'trilat_error_'+str(i)] = np.round(barerror,tolera_error)
# error trilatera hacia gps
dx = baricentro[0] - c_este
dy = baricentro[1] - c_norte
error_gps = np.sqrt(dx**2+dy**2)
tabla.loc[cadapunto,'trilat_gps_error_'+str(i)] = np.round(error_gps,tolera_error)
tabla.loc[cadapunto,'trilat_gps_dx_'+str(i)] = np.round(dx,tolera_error)
tabla.loc[cadapunto,'trilat_gps_dy_'+str(i)] = np.round(dy,tolera_error)
# cuenta error si encuentra primera vez
cond1 = np.isnan(tabla['encontrado'][cadapunto])
cond2 = len(tabla['interv_fuera'][cadapunto])>0
k = int(cond2) # intervalo extendido
if cond1:
tabla.loc[cadapunto,'encontrado'] = i
ubicados[k,i] = ubicados[k,i]+1
sumatrilat[k,i] = sumatrilat[k,i] + np.round(barerror,tolera_error)
sumagps[k,i] = sumagps[k,i] + np.round(error_gps,tolera_error)
sumagpsdx[k,i] = sumagpsdx[k,i] + np.round(np.abs(dx),tolera_error)
sumagpsdy[k,i] = sumagpsdy[k,i] + np.round(np.abs(dy),tolera_error)
# contabiliza errores de localizados
ubicasuma = np.zeros(2)
ubicasumporc = np.zeros(2)
ubicaporc = np.zeros(shape=(2,3))
for k in range(0,2,1):
ubicados[k,3] = int(np.sum(ubicados[k,0:3]))
ubicasuma[k] = np.sum(ubicados[k,0:3])
ubicasumporc[k] = np.round(100*ubicasuma[k]/ubicados[k,3],1)
for i in range(0,3,1):
ubicaporc[k,i] = np.round(100*ubicados[k,i]/ubicasuma[k],1)
if ubicados[k,i]>0:
sumatrilat[k,i] = np.round(sumatrilat[k,i]/ubicados[k,i],1)
sumagps[k,i] = np.round(sumagps[k,i]/ubicados[k,i],1)
sumagpsdx[k,i] = np.round(sumagpsdx[k,i]/ubicados[k,i],1)
sumagpsdy[k,i] = np.round(sumagpsdy[k,i]/ubicados[k,i],1)
# SALIDA
print('Errores localizacion')
import prettytable as ptt
print('Errores estimado: Cota-Trilatera-polígono y Trilatera_vs_GPS')
mostrar = ptt.PrettyTable(['punto','i*std','fuera','trilat',
'gps','gps_dx','gps_dy',
'u_d1','u_d2','u_d3'])
for cadapunto in tabla.index:
# selecciona puntos a mostrar
encontrado = tabla['encontrado'][cadapunto]
esgrupo = tabla['grupo'][cadapunto]
interv_fuera = int(len(tabla['interv_fuera'][cadapunto])!=0)
cond1 = not(np.isnan(encontrado))
cond2 = esgrupo in mostrargrp
cond3 = interv_fuera in mostrarfuera
if cond1 and cond2 and cond3:
encontrado = int(encontrado)
mostrar.add_row([cadapunto,
str(int(tabla['encontrado'][cadapunto])),
tabla['interv_fuera'][cadapunto],
str(tabla['trilat_error_'+str(encontrado)][cadapunto]),
str(tabla['trilat_gps_error_'+str(encontrado)][cadapunto]),
str(tabla['trilat_gps_dx_'+str(encontrado)][cadapunto]),
str(tabla['trilat_gps_dy_'+str(encontrado)][cadapunto]),
tabla['usar_d1'][cadapunto],
tabla['usar_d2'][cadapunto],
tabla['usar_d3'][cadapunto]])
print(mostrar)
print('puntos con medidas: ',procesados)
loc_alg = np.round(100*np.sum(ubicados[:,3])/procesados,1)
print('localizados con algoritmo: ',
np.sum(ubicados[:,3]),
' , '+str(loc_alg)+'%' )
print('Errores Promedio:')
resumen = ptt.PrettyTable(['error+i*std','cant','%',
'trilat','gps','gps_dx',
'gps_dy'])
for k in range(0,2,1):
if k == 0:
texto = 'dentro'
if k == 1:
texto = 'fuera'
resumen.add_row(['interv_'+texto,ubicados[k,3],'',
'','','',''])
for i in range(0,3,1):
resumen.add_row(['error_'+str(i),ubicados[k,i],
str(ubicaporc[k,i])+'%',
sumatrilat[k,i],sumagps[k,i],
sumagpsdx[k,i],sumagpsdy[k,i],
])
print(resumen)
# salida hacia archivo
tabla.to_csv(arch_trilatera)
# Grafica ubicados ------------
# Referencias para gráfica
grupo = ['FIEC' ,'FCNM' ,'RECT','CIRC']
colores = ['green','orange','grey','magenta']
tipo = ['punto','1m' ,'gtw','dispositivo']
marcas = [ 'o','D' ,'D' ,'*' ]
colorstd = ['lightblue', 'lightgreen','orange']
colorlin = ['lightblue', 'lightgreen','orange']
figura,grafica = plt.subplots()
# balizas
for unabaliza in coordbaliza:
g_este = coordbaliza[unabaliza][0]
g_norte = coordbaliza[unabaliza][1]
grafica.scatter(g_este,g_norte,
color = 'red',
marker = 'D',
label = cadapunto)
grafica.annotate(unabaliza,
(g_este,g_norte))
# Puntos
for cadapunto in tabla.index:
g_este = tabla['c_este'][cadapunto]
g_norte = tabla['c_norte'][cadapunto]
# selecciona puntos a mostrar
encontrado = tabla['encontrado'][cadapunto]
esgrupo = tabla['grupo'][cadapunto]
interv_fuera = int(len(tabla['interv_fuera'][cadapunto])!=0)
cond1 = not(np.isnan(encontrado))
cond2 = esgrupo in mostrargrp
cond3 = interv_fuera in mostrarfuera
if cond1 and cond2 and cond3:
encontrado = int(encontrado)
p_este = tabla['trilat_este_'+str(encontrado)][cadapunto]
p_norte = tabla['trilat_norte_'+str(encontrado)][cadapunto]
grafica.scatter(p_este,p_norte,
color = colorstd[encontrado],
label = cadapunto)
grafica.plot([p_este,g_este],
[p_norte,g_norte],
color = colorstd[encontrado],
linestyle='dotted')
grafica.scatter(g_este,g_norte,
color = 'blue',
label = cadapunto)
grafica.annotate(cadapunto,(g_este,g_norte),
color='blue')
grafica.set_xlabel('UTM_este')
grafica.set_ylabel('UTM_norte')
grafica.grid()
grafica.set_title('Puntos Ubicados')
plt.show()