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()