5.2 Localiza por Trilateración – Intervalos. Algoritmo Python

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