5.4 Localiza por Trilateración – Sectores e Intervalos. Algoritmo Python

Los sectores en una baliza se incorporan en el algoritmo como la estimación de ubicación del punto usando las otras balizas.

Para un punto de ejemplo, dado que baliza ‘gtwRECT’ tiene sectores, se usan las otras balizas ‘gtwFIEC’ y ‘gtwFCNM’ para estimar la ubicación y el ángulo que permite aproximar el sector donde se ubicaría el punto.

Este sector se usa para aplicar la ecuación que corresponde al  ‘gtwFIEC’.

El desarrollo continua desde la sección anterior con solo intervalos y se incorpora el concepto.

Algoritmo 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+'UbicaUsarSector01.txt'
arch_ecuaciones = 'rsmP06_ecuacionSector01.json'

# archivos de salida
arch_trilatera = 'rsmP07_'+medida+'trilateraSector01.txt'

# Parametros de grafica
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'}
# Parámetros de grafica
tipograf   = '2D'  # '2D','' sin grafica

# 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
tabla['ensector'] = ''
# 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]

        unsector = 's0'
        if not(np.isnan(p_rssi)):
            ecuacion_rssi = ecuacion[unabaliza][unsector]
            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,:])

# Revisa un sector_estimado
columnas = list(tabla.keys())
for cadapunto in tabla.index:
    for cualbaliza in baliza:
        sector_baliza = 'sector_'+cualbaliza
        unsector = 0
        if sector_baliza in columnas:
            unsector = tabla['sector_'+cualbaliza][cadapunto]
        if unsector!=0:
            # evalua en sector
            donde = baliza_key.index(cualbaliza)
            otrasbalizas = baliza_key.copy()
            otrasbalizas.pop(donde)

            # coordenadas de baliza
            coord_otrasbalizas = {}
            for cadaotrabaliza in otrasbalizas:
                cualotrabaliza = baliza[cadaotrabaliza]
                coord_otrasbalizas[cadaotrabaliza] = [tabla['c_este'][cualotrabaliza]]
                coord_otrasbalizas[cadaotrabaliza].append(tabla['c_norte'][cualotrabaliza])
            
            # cada punto a i*std
            punto = {}
            sumabaricentro = np.nan
            for i in range(0,3,1):
                punto[i] = {}
                cuenta = 0
                for cadaotrabaliza in otrasbalizas:
                    punto[i][cadaotrabaliza] = tabla['dist'+str(i)+'_'+cadaotrabaliza][cadapunto]
                    # si existe distancia
                    if not(np.isnan(punto[i][cadaotrabaliza])):
                        cuenta = cuenta + 1
                # Hay 3 mediciones a baliza
                if cuenta==2:
                    localiza   = girni.trilatera(punto[i],
                                                 coord_otrasbalizas,
                                                 tolera = 10e-4)
                    baricentro = localiza['baricentro']
                    barerror   = localiza['barerror']
                    poligono   = localiza['poligono']
                    sumabaricentro = np.sum(baricentro)
                    if not(np.isnan(sumabaricentro)):
                        break
            if not(np.isnan(sumabaricentro)):
                # coordenadas baliza para identificar ángulo
                b_este  = tabla['c_este'][baliza[cualbaliza]]
                b_norte = tabla['c_norte'][baliza[cualbaliza]]
                # coordenadas del punto entre otras balizas
                p_este  = baricentro[0]
                p_norte = baricentro[1]
                dx = p_este-b_este
                dy = p_norte-b_norte
                theta = np.arctan2(dy,dx)
                if theta<0:
                    theta = theta + 2*np.pi
                sectores   =  ecuacion[baliza[cualbaliza]]['sector_rad']
                nsectores  = len(sectores)
                otrosector = ''
                for i in range(0,nsectores-1,1):
                    a = sectores[i]
                    b = sectores[i+1]
                    if theta>=a and theta<b:
                        otrosector = i+1
                if otrosector !='':
                    tabla.loc[cadapunto,'sector_'+cualbaliza] = otrosector
                    
                    # recalcula distancia con sector
                    columna = medida+'_'+modo+'_'+cualbaliza
                    p_rssi = tabla[columna][cadapunto]
                    ecuacion_rssi = ecuacion[baliza[cualbaliza]]['s'+str(otrosector)]
                    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

# trilateración. localiza cada punto
# 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]
    # coordenadas GPS del punto
    c_este  = tabla['c_este'][cadapunto]
    c_norte = tabla['c_norte'][cadapunto]
    if esgrupo in mostrargrp:
        # 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','sect_d1','sect_d2','sect_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['sector_d1'][cadapunto],
                         tabla['sector_d2'][cadapunto],
                         tabla['sector_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']

if tipograf == '2D':
    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()