1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
|
"""
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()
|
