Skip to content

Commit 845f65f

Browse files
danineamatidanineamati
committed
Added splatting and unsplatting support
1 parent 29ace4c commit 845f65f

2 files changed

Lines changed: 197 additions & 18 deletions

File tree

‎gnss_lib_py/utils/dop.py‎

Lines changed: 97 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,23 @@
1212
from gnss_lib_py.utils.coordinates import el_az_to_enu_unit_vector
1313

1414

15+
def get_enu_dop_labels():
16+
"""
17+
Helper function to get the DOP labels.
18+
19+
Returns
20+
-------
21+
dop_labels : list
22+
List of strings for the DOP labels.
23+
"""
24+
25+
dop_labels = ['ee', 'en', 'eu', 'et',
26+
'nn', 'nu', 'nt',
27+
'uu', 'ut',
28+
'tt']
29+
return dop_labels
30+
31+
1532
def get_dop(navdata, **which_dop):
1633
"""Get DOP from navdata.
1734
@@ -92,18 +109,15 @@ def get_dop(navdata, **which_dop):
92109

93110
# Special handling for splatting the dop_matrix
94111
if which_dop['dop_matrix']:
95-
dop_matrix_splat = []
96112

97-
dop_labels = ['ee', 'en', 'eu', 'et',
98-
'nn', 'nu', 'nt',
99-
'uu', 'ut',
100-
'tt']
113+
dop_labels = get_enu_dop_labels()
101114

115+
dop_matrix_splat = []
116+
102117
for dop_matrix in dop_out['dop_matrix']:
103-
dop_matrix_splat.append(dop_matrix[(0, 0, 0, 0, 1, 1, 1, 2, 2, 3),
104-
(0, 1, 2, 3, 1, 2, 3, 2, 3, 3)])
118+
dop_matrix_splat.append(splat_dop_matrix(dop_matrix))
105119

106-
# Convert to numpy array
120+
# Convert entire array across time to numpy array
107121
dop_matrix_splat = np.array(dop_matrix_splat)
108122
assert dop_matrix_splat.shape == (len(times), len(dop_labels)), \
109123
f"DOP matrix splatted to {dop_matrix_splat.shape}."
@@ -115,6 +129,81 @@ def get_dop(navdata, **which_dop):
115129
return dop_navdata
116130

117131

132+
def splat_dop_matrix(dop_matrix):
133+
"""
134+
Splat the DOP matrix into a 1D array. Note that the dop matrix output
135+
is splatted across entries following
136+
the behavior below:
137+
[[EE, EN, EU, ET],
138+
[NE, NN, NU, NT],
139+
[UE, UN, UU, UT],
140+
[TE, TN, TU, TT]] (16 values in 2D array)
141+
is stored as
142+
[EE, EN, EU, ET,
143+
NN, NU, NT,
144+
UU, UT,
145+
TT] (10 values in 1D array),
146+
recognizing that the dop matrix is symmetric.
147+
148+
Parameters
149+
----------
150+
dop_matrix : np.ndarray (4, 4)
151+
DOP matrix in ENU coordinates.
152+
153+
Returns
154+
-------
155+
dop_splat : np.ndarray (10,)
156+
DOP matrix splatted into a 1D array.
157+
"""
158+
159+
# Splat the DOP matrix
160+
dop_splat = dop_matrix[(0, 0, 0, 0, 1, 1, 1, 2, 2, 3),
161+
(0, 1, 2, 3, 1, 2, 3, 2, 3, 3)]
162+
163+
return np.array(dop_splat)
164+
165+
166+
def unsplat_dop_matrix(dop_splat):
167+
"""
168+
Un-splat the DOP matrix from a 1D array. Note that the dop matrix output
169+
is unsplatted across entries following the behavior below:
170+
[EE, EN, EU, ET,
171+
NN, NU, NT,
172+
UU, UT,
173+
TT] (10 values in 1D array)
174+
is unsplatted to
175+
[[EE, EN, EU, ET],
176+
[NE, NN, NU, NT],
177+
[UE, UN, UU, UT],
178+
[TE, TN, TU, TT]] (16 values in 2D array)
179+
recognizing that the dop matrix is symmetric.
180+
181+
Parameters
182+
----------
183+
dop_splat : np.ndarray (10,)
184+
DOP matrix splatted into a 1D array.
185+
186+
Returns
187+
-------
188+
dop_matrix : np.ndarray (4, 4)
189+
DOP matrix in ENU coordinates.
190+
"""
191+
192+
# Un-splat the DOP matrix
193+
dop_matrix = np.zeros((4, 4))
194+
195+
splat_rows = (0, 0, 0, 0, 1, 1, 1, 2, 2, 3)
196+
splat_cols = (0, 1, 2, 3, 1, 2, 3, 2, 3, 3)
197+
198+
# Fill in the upper triangle of the DOP matrix
199+
dop_matrix[splat_rows, splat_cols] = dop_splat
200+
# Fill in the lower triangle of the DOP matrix
201+
# (Note that the diagonal is filled in again, but that's okay.)
202+
dop_matrix[splat_cols, splat_rows] = dop_splat
203+
204+
return dop_matrix
205+
206+
118207

119208
def calculate_enu_dop_matrix(derived):
120209
"""

‎tests/utils/test_dop.py‎

Lines changed: 100 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -15,8 +15,12 @@
1515
import copy
1616

1717
from gnss_lib_py.navdata.navdata import NavData
18+
from gnss_lib_py.navdata.operations import loop_time
19+
1820
from gnss_lib_py.utils.dop import \
19-
get_dop, calculate_dop, _calculate_enut_matrix
21+
get_enu_dop_labels, get_dop, calculate_dop, \
22+
splat_dop_matrix, unsplat_dop_matrix, \
23+
_calculate_enut_matrix
2024

2125

2226
#####################################################################
@@ -218,10 +222,7 @@ def test_simple_get_dop(navdata, expected_dop, which_dop):
218222

219223
if 'dop_matrix' in dop_navdata:
220224
# Handle the splatting of the DOP matrix
221-
dop_labels = ['ee', 'en', 'eu', 'et',
222-
'nn', 'nu', 'nt',
223-
'uu', 'ut',
224-
'tt']
225+
dop_labels = get_enu_dop_labels()
225226

226227
for label in dop_labels:
227228
assert f"dop_{label}" in dop_navdata.rows
@@ -235,7 +236,61 @@ def test_simple_get_dop(navdata, expected_dop, which_dop):
235236
dop_navdata[f"dop_{dop_labels[ind]}"],
236237
expected_dop['dop_matrix'][r, c])
237238
ind += 1
238-
239+
240+
241+
@pytest.mark.parametrize('navdata, expected_dop',
242+
[
243+
(lazy_fixture('simple_sat_scenario'),
244+
lazy_fixture('simple_sat_expected_dop'))
245+
])
246+
def test_splat_dop_matrix(navdata, expected_dop):
247+
"""
248+
Test that the splat_dop_matrix function works correctly.
249+
"""
250+
# Perform the function under test
251+
dop_matrix = calculate_dop(navdata)['dop_matrix']
252+
dop_matrix_splat = splat_dop_matrix(dop_matrix)
253+
254+
# Check the splatting is correct
255+
# edopmat is 'expected dop matrix'
256+
edopmat = expected_dop['dop_matrix']
257+
expected_dop_matrix_splat = np.array(
258+
[
259+
edopmat[0, 0], edopmat[0, 1], edopmat[0, 2], edopmat[0, 3],
260+
edopmat[1, 1], edopmat[1, 2], edopmat[1, 3],
261+
edopmat[2, 2], edopmat[2, 3],
262+
edopmat[3, 3]
263+
])
264+
265+
np.testing.assert_array_almost_equal(
266+
dop_matrix_splat, expected_dop_matrix_splat)
267+
268+
269+
@pytest.mark.parametrize('navdata, expected_dop',
270+
[
271+
(lazy_fixture('simple_sat_scenario'),
272+
lazy_fixture('simple_sat_expected_dop'))
273+
])
274+
def test_unsplat_dop_matrix(navdata, expected_dop):
275+
"""
276+
Test that the unsplat_dop_matrix function works correctly.
277+
"""
278+
# Perform the function under test
279+
dop_matrix = calculate_dop(navdata)['dop_matrix']
280+
dop_matrix_splat = splat_dop_matrix(dop_matrix)
281+
dop_matrix_unsplat = unsplat_dop_matrix(dop_matrix_splat)
282+
283+
# Check that the unsplatted matrix is symmetric
284+
np.testing.assert_array_almost_equal(
285+
dop_matrix_unsplat.T, dop_matrix_unsplat)
286+
287+
# Check the unsplatting values are correct
288+
np.testing.assert_array_almost_equal(
289+
dop_matrix_unsplat, expected_dop['dop_matrix'])
290+
np.testing.assert_array_almost_equal(
291+
dop_matrix_unsplat, dop_matrix)
292+
293+
239294
#############################################
240295
# Singularity issues and edge cases
241296

@@ -371,10 +426,7 @@ def test_dop_across_time_with_selection(navdata, which_dop):
371426

372427
if 'dop_matrix' in dop_navdata:
373428
# Handle the splatting of the DOP matrix
374-
dop_labels = ['ee', 'en', 'eu', 'et',
375-
'nn', 'nu', 'nt',
376-
'uu', 'ut',
377-
'tt']
429+
dop_labels = get_enu_dop_labels()
378430

379431
for label in dop_labels:
380432
assert f"dop_{label}" in dop_navdata.rows
@@ -406,3 +458,41 @@ def test_dop_across_time_with_selection(navdata, which_dop):
406458
dop_navdata['TDOP'],
407459
np.sqrt(dop_navdata['dop_tt']))
408460

461+
462+
@pytest.mark.parametrize('navdata',
463+
[
464+
lazy_fixture('android_derived')
465+
])
466+
def test_splat_unsplat_dop_matrix_across_time(navdata):
467+
"""
468+
Test that we can splat and unsplat the DOP matrix across time.
469+
"""
470+
471+
# Run through the data, calculate the DOP, and store as navdata
472+
dop_navdata = get_dop(navdata, dop_matrix=True)
473+
474+
# Check we have the dop_matrix entries
475+
dop_labels = get_enu_dop_labels()
476+
477+
for label in dop_labels:
478+
assert f"dop_{label}" in dop_navdata.rows
479+
480+
for _, _, dop_navdata_subset in loop_time(dop_navdata, 'gps_millis'):
481+
# Extract the dop matrix
482+
dop_matrix_splat = np.array([dop_navdata_subset[f"dop_{label}"]
483+
for label in dop_labels])
484+
485+
# Unsplat the DOP matrix
486+
dop_matrix_unsplat = unsplat_dop_matrix(dop_matrix_splat)
487+
488+
# Check that the unsplatted matrix is symmetric
489+
np.testing.assert_array_almost_equal(
490+
dop_matrix_unsplat.T, dop_matrix_unsplat)
491+
492+
# Resplat the DOP matrix
493+
dop_matrix_resplat = splat_dop_matrix(dop_matrix_unsplat)
494+
495+
# Check the unsplatting is correct
496+
np.testing.assert_array_almost_equal(
497+
dop_matrix_splat, dop_matrix_resplat)
498+

0 commit comments

Comments
 (0)