Coordinate Conversions

import numpy as np
import gnss_lib_py as glp

ECEF coordinates for positions can be converted to LLA and back by using functions defined in utils/coordinates.py

# Using the LLA coordinates the Aero/Astro department at Stanford University
x_lla = np.array([[37.427112], [-122.1764146], [16]])
print('Converted ECEF coordinates')
print(glp.geodetic_to_ecef(x_lla))

x_ecef = np.array([[-2700628], [-4292443], [3855152]])
print('Converted LLA coordinates')
print(glp.ecef_to_geodetic(x_ecef))

Converted ECEF coordinates
[[-2700628.97971166]
 [-4292443.61165747]
 [ 3855152.80233124]]
Converted LLA coordinates
[[  37.42711195]
 [-122.17640891]
 [  14.68637162]]

When using a NavData instance with multiple ECEF coordinates, the ECEF coordinates can be accessed from the NavData and then passed to the coordinate conversion function.

The obtained values can be assigned as rows to the NavData instance where needed.

# load Android Google Challenge data
glp.make_dir("../data")
!wget https://raw.githubusercontent.com/Stanford-NavLab/gnss_lib_py/main/data/unit_test/google_decimeter_2022/device_gnss.csv --quiet -nc -O "../data/device_gnss.csv"
navdata = glp.AndroidDerived2022("../data/device_gnss.csv")
pos_rx_ecef = navdata[["x_rx_m","y_rx_m","z_rx_m"]]
pos_rx_lla = glp.ecef_to_geodetic(pos_rx_ecef)
# can also use pos_rx_lla = glp.ecef_to_geodetic(navdata[["x_rx_m","y_rx_m","z_rx_m"]])
print(pos_rx_lla[:, :4])

[[  37.39582179   37.39582179   37.39582179   37.39582179]
 [-122.10293436 -122.10293436 -122.10293436 -122.10293436]
 [   1.06654795    1.06654795    1.06654795    1.06654795]]

The output maintains the shape of the input, so in this case the first row is latitude, second is longitude, and third is altitude above the WGS-84 datum. These rows can be assigned to the NavData by accessing the array

Local NED Frame Conversions

A local North-East-Down (NED) frame of reference can be instantiated by defining the point at the origin of the coordinate frame using either its ECEF position or LLA (w.r.t the WGS-84 ellipsoid) coordinates

# Instantiate using LLA coordinates
local_frame = glp.LocalCoord.from_geodetic(x_lla)
print('NED to ECEF conversion matrix for initialized local frame')
print(local_frame.ned_to_ecef_matrix)

local_frame = glp.LocalCoord.from_ecef(x_ecef)
print('NED to ECEF conversion matrix for initialized local frame')
print(local_frame.ned_to_ecef_matrix)

NED to ECEF conversion matrix for initialized local frame
[[ 0.32364473  0.84641245  0.42289485]
 [ 0.51440859 -0.5325279   0.67215908]
 [ 0.79412713  0.         -0.60775168]]
NED to ECEF conversion matrix for initialized local frame
[[ 0.32364468  0.8464125   0.42289479]
 [ 0.51440862 -0.53252782  0.67215913]
 [ 0.79412713  0.         -0.60775168]]

Once initialized, the local_frame can be used to convert position vectors to NED from ECEF or LLA and vice-versa

ned = local_frame.ecef_to_ned(x_ecef)
print('The converted NED coordinates are')
print(ned)

ecef = local_frame.ned_to_ecef(ned)
print('The converted ECEF coordinates are')
print(ecef)

lla = local_frame.ned_to_geodetic(ned)
print('The converted LLA coordinates are')
print(lla)

The converted NED coordinates are
[[0.]
 [0.]
 [0.]]
The converted ECEF coordinates are
[[-2700628.]
 [-4292443.]
 [ 3855152.]]
The converted LLA coordinates are
[[  37.42711195]
 [-122.17640891]
 [  14.68637162]]

The local_frame can also be used to convert free vectors in the NED frame of reference to free vectors in the ECEF frame of reference

v_vect = np.array([[1], [0], [0]])
v_ned = local_frame.ned_to_ecefv(v_vect)
print('The converted free vector in ECEF is')
print(v_ned)
v_ecef = local_frame.ecef_to_nedv(v_ned)
print('The converted free vector in NED is ')
print(v_ecef)

The converted free vector in ECEF is
[[0.32364468]
 [0.51440862]
 [0.79412713]]
The converted free vector in NED is
[[1.]
 [0.]
 [0.]]

If you want to use ENU local frame of reference instead, the resultant coordinates can be matrix multipled by the corresponding rotation matrix.

ned_to_enu_rot_mat = np.array([[0, 1, 0],
                               [1, 0, 0],
                               [0, 0, -1]])

Elevation and Aziumth from ECEF Positions

Find elevation and azimuth angle from receiver and satellite ECEF positions.

# For this case, only use data from first timestep
navdata_subset = navdata.where("gps_millis",navdata["gps_millis",0])

To calculate the elevation and azimuth, pass in the receiver and satellites’ ECEF positions.

pos_sv_m = navdata_subset[["x_sv_m","y_sv_m","z_sv_m"]]
pos_rx_m = navdata_subset[["x_rx_m","y_rx_m","z_rx_m"],0].reshape(-1,1)

calculated_el_az = glp.ecef_to_el_az(pos_rx_m,pos_sv_m)
truth_el_az = navdata_subset[["el_sv_deg","az_sv_deg"]]

We can now compare the calculated elevation and azimuth with their respective “truth” values included in the Google Decimeter Challenge 2022 dataset.

for sat_idx in range(3):
    print(f"SV ID: {int(navdata_subset['sv_id',sat_idx])}")
    print(f"Calculated elevation: {calculated_el_az[0, sat_idx]}, Truth elevation: {truth_el_az[0, sat_idx]}")
    print(f"Calculated azimuth: {calculated_el_az[1, sat_idx]}, Truth azimuth: {truth_el_az[1, sat_idx]}")

SV ID: 2
Calculated elevation: 62.44920476228993, Truth elevation: 62.44920476227796
Calculated azimuth: 43.7730072870086, Truth azimuth: 43.77300728698665
SV ID: 5
Calculated elevation: 27.169886307916364, Truth elevation: 27.16988630793112
Calculated azimuth: 152.993936423783, Truth azimuth: 152.99393642377913
SV ID: 6
Calculated elevation: 25.45246950401311, Truth elevation: 25.45246950400123
Calculated azimuth: 44.140622789726976, Truth azimuth: 44.14062278972148