diff --git a/src/prx/rinex_nav/evaluate.py b/src/prx/rinex_nav/evaluate.py index b221cc2..4034848 100644 --- a/src/prx/rinex_nav/evaluate.py +++ b/src/prx/rinex_nav/evaluate.py @@ -315,25 +315,26 @@ def orbital_plane_to_earth_centered_cartesian(eph): pass +@timeit def handle_bds_geos(eph): # Do special rotation from inertial to BDCS (ECEF) frame for Beidou GEO satellites, see # Beidou_ICD_B3I_v1.0, Table 5-11 geos = eph[eph.is_bds_geo] if geos.empty: - return + return eph P_GK = np.reshape(geos[["X_k", "Y_k", "Z_k"]].to_numpy(), (-1, 1)) V_GK = np.reshape(geos[["dX_k", "dY_k", "dZ_k"]].to_numpy(), (-1, 1)) - z_angles = geos.OmegaEarthIcd_rps * geos.t_k + z_angles = geos["OmegaEarthIcd_rps"] * geos["t_k"] rotation_matrices = [] + x_angle = util.deg_2_rad(-5.0) + Rx = np.array( + [ + [1, 0, 0], + [0, np.cos(x_angle), np.sin(x_angle)], + [0, -np.sin(x_angle), np.cos(x_angle)], + ] + ) for i, z_angle in enumerate(z_angles): - x_angle = util.deg_2_rad(-5.0) - Rx = np.array( - [ - [1, 0, 0], - [0, np.cos(x_angle), np.sin(x_angle)], - [0, -np.sin(x_angle), np.cos(x_angle)], - ] - ) Rz = np.array( [ [np.cos(z_angle), np.sin(z_angle), 0], @@ -356,16 +357,15 @@ def handle_bds_geos(eph): geos["dZ_k"] = V_K_frozen[:, 2] # Add term due to ECEFs angular velocity w.r.t. the frozen frame + # Leverage the fact that there are only BDS GEOs + assert geos["OmegaEarthIcd_rps"].nunique() == 1 + OmegaEarthIcd_rps = geos["OmegaEarthIcd_rps"].iloc[0] + geos[["dX_k", "dY_k", "dZ_k"]] += np.cross( + np.array([0, 0, -OmegaEarthIcd_rps]), geos[["X_k", "Y_k", "Z_k"]].to_numpy() + ) - def frozen_to_rotating_bdcs(row): - p = np.array([row["X_k"], row["Y_k"], row["Z_k"]]) - v_frozen = np.array([row["dX_k"], row["dY_k"], row["dZ_k"]]) - v_rotating = v_frozen + np.cross(np.array([0, 0, -row.OmegaEarthIcd_rps]), p) - row[["dX_k", "dY_k", "dZ_k"]] = v_rotating - return row - - geos = geos.apply(frozen_to_rotating_bdcs, axis=1) eph[eph.is_bds_geo] = geos + return eph # Adapted from gnss_lib_py's find_sat() @@ -392,7 +392,7 @@ def kepler_orbit_position_and_velocity(eph): ) position_in_orbital_plane(eph) orbital_plane_to_earth_centered_cartesian(eph) - handle_bds_geos(eph) + eph = handle_bds_geos(eph) eph = eph.rename( columns={ "X_k": "sat_pos_x_m", diff --git a/src/prx/test/benchmark.py b/src/prx/test/benchmark.py index f5483e4..a50b7e0 100644 --- a/src/prx/test/benchmark.py +++ b/src/prx/test/benchmark.py @@ -86,7 +86,7 @@ def main(ram: bool, obs_file: Path, warm_parser_cache: bool): configure_logging("DEBUG") cases = generate_inputs( - n_steps=10, + n_steps=6, obs_file=obs_file, root=obs_file.parent / "benchmark_datasets" if obs_file is not None else None, )