mirror of https://github.com/gnss-sdr/gnss-sdr
135 lines
5.3 KiB
Python
135 lines
5.3 KiB
Python
"""
|
|
plotNavigation.py
|
|
|
|
Function plots variations of coordinates over time and a 3D position
|
|
plot. It plots receiver coordinates in UTM system or coordinate offsets if
|
|
the true UTM receiver coordinates are provided.
|
|
|
|
Irene Pérez Riega, 2023. iperrie@inta.es
|
|
|
|
plotNavigation(navSolutions, settings, plot_skyplot)
|
|
|
|
Args:
|
|
navSolutions - Results from navigation solution function. It
|
|
contains measured pseudoranges and receiver
|
|
coordinates.
|
|
settings - Receiver settings. The true receiver coordinates
|
|
are contained in this structure.
|
|
plot_skyplot - If == 1 then use satellite coordinates to plot the
|
|
satellite positions (not implemented yet TO DO)
|
|
|
|
Modifiable in the file:
|
|
fig_path - Path where plots will be save
|
|
|
|
-----------------------------------------------------------------------------
|
|
|
|
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
|
This file is part of GNSS-SDR.
|
|
|
|
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
|
|
SPDX-License-Identifier: GPL-3.0-or-later
|
|
|
|
-----------------------------------------------------------------------------
|
|
"""
|
|
|
|
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
import os
|
|
|
|
|
|
def plotNavigation(navSolutions, settings, plot_skyplot=0):
|
|
|
|
# ---------- CHANGE HERE:
|
|
fig_path = '/home/labnav/Desktop/TEST_IRENE/PLOTS/PlotNavigation'
|
|
|
|
if not os.path.exists(fig_path):
|
|
os.makedirs(fig_path)
|
|
|
|
if navSolutions:
|
|
if (np.isnan(settings['true_position']['E_UTM']) or
|
|
np.isnan(settings['true_position']['N_UTM']) or
|
|
np.isnan(settings['true_position']['U_UTM'])):
|
|
|
|
# Compute mean values
|
|
ref_coord = {
|
|
'E_UTM': np.nanmean(navSolutions['E_UTM']),
|
|
'N_UTM': np.nanmean(navSolutions['N_UTM']),
|
|
'U_UTM': np.nanmean(navSolutions['U_UTM'])
|
|
}
|
|
|
|
mean_latitude = np.nanmean(navSolutions['latitude'])
|
|
mean_longitude = np.nanmean(navSolutions['longitude'])
|
|
mean_height = np.nanmean(navSolutions['height'])
|
|
ref_point_lg_text = (f"Mean Position\nLat: {mean_latitude}º\n"
|
|
f"Long: {mean_longitude}º\n"
|
|
f"Hgt: {mean_height:+6.1f}")
|
|
|
|
else:
|
|
# Compute the mean error for static receiver
|
|
ref_coord = {
|
|
'E_UTM': settings.truePosition['E_UTM'],
|
|
'N_UTM': settings.truePosition['N_UTM'],
|
|
'U_UTM': settings.truePosition['U_UTM']
|
|
}
|
|
|
|
mean_position = {
|
|
'E_UTM': np.nanmean(navSolutions['E_UTM']),
|
|
'N_UTM': np.nanmean(navSolutions['N_UTM']),
|
|
'U_UTM': np.nanmean(navSolutions['U_UTM'])
|
|
}
|
|
|
|
error_meters = np.sqrt(
|
|
(mean_position['E_UTM'] - ref_coord['E_UTM']) ** 2 +
|
|
(mean_position['N_UTM'] - ref_coord['N_UTM']) ** 2 +
|
|
(mean_position['U_UTM'] - ref_coord['U_UTM']) ** 2)
|
|
|
|
ref_point_lg_text = (f"Reference Position, Mean 3D error = "
|
|
f"{error_meters} [m]")
|
|
|
|
#Create plot and subplots
|
|
plt.figure(figsize=(1920 / 120, 1080 / 120))
|
|
plt.clf()
|
|
plt.title('Navigation solutions',fontweight='bold')
|
|
|
|
ax1 = plt.subplot(4, 2, (1, 4))
|
|
ax2 = plt.subplot(4, 2, (5, 7), projection='3d')
|
|
ax3 = plt.subplot(4, 2, (6, 8), projection='3d')
|
|
|
|
# (ax1) Coordinate differences in UTM system from reference point
|
|
ax1.plot(np.vstack([navSolutions['E_UTM'] - ref_coord['E_UTM'],
|
|
navSolutions['N_UTM'] - ref_coord['N_UTM'],
|
|
navSolutions['U_UTM'] - ref_coord['U_UTM']]).T)
|
|
ax1.set_title('Coordinates variations in UTM system', fontweight='bold')
|
|
ax1.legend(['E_UTM', 'N_UTM', 'U_UTM'])
|
|
ax1.set_xlabel(f"Measurement period: {settings['navSolPeriod']} ms")
|
|
ax1.set_ylabel('Variations (m)')
|
|
ax1.grid(True)
|
|
ax1.axis('tight')
|
|
|
|
# (ax2) Satellite sky plot
|
|
if plot_skyplot: #todo posicion de los satelites
|
|
skyPlot(ax2, navSolutions['channel']['az'],
|
|
navSolutions['channel']['el'],
|
|
navSolutions['channel']['PRN'][:, 0])
|
|
ax2.set_title(f'Sky plot (mean PDOP: '
|
|
f'{np.nanmean(navSolutions["DOP"][1, :]):.1f})',
|
|
fontweight='bold')
|
|
|
|
# (ax3) Position plot in UTM system
|
|
ax3.scatter(navSolutions['E_UTM'] - ref_coord['E_UTM'],
|
|
navSolutions['N_UTM'] - ref_coord['N_UTM'],
|
|
navSolutions['U_UTM'] - ref_coord['U_UTM'], marker='+')
|
|
ax3.scatter([0], [0], [0], color='r', marker='+', linewidth=1.5)
|
|
ax3.view_init(0, 90)
|
|
ax3.set_box_aspect([1, 1, 1])
|
|
ax3.grid(True, which='minor')
|
|
ax3.legend(['Measurements', ref_point_lg_text])
|
|
ax3.set_title('Positions in UTM system (3D plot)',fontweight='bold')
|
|
ax3.set_xlabel('East (m)')
|
|
ax3.set_ylabel('North (m)')
|
|
ax3.set_zlabel('Upping (m)')
|
|
|
|
plt.tight_layout()
|
|
plt.savefig(os.path.join(fig_path, 'measures_UTM.png'))
|
|
plt.show()
|