"""Time-domain model of the receiver's current GPS WLS observation path. The intent is not to be a general GNSS simulator. This module deliberately mirrors the conventions used by gps_system/src/gps_ip.c and NavEquations.c: * corrected_abs = raw_abs - rem_code_phase / CODE_FREQ_BASIS * modulo-32 ABS_SAMPLE values are unwrapped around the newest channel * obs_i/c = (corrected_abs_i - corrected_abs_ref) / RF_HZ - (transmit_i - transmit_ref) * satellite clock correction is added to the observation * [x, y, z, common_bias_m] are solved together with URA/elevation weights The synthetic signal generator uses one common navigation-bit transmit epoch. Different satellite path delays therefore produce the per-channel arrival times that the real ABS_SAMPLE hardware observes. """ from __future__ import annotations from dataclasses import dataclass from typing import Iterable import numpy as np C = 299_792_458.0 RF_HZ = 16_367_981.0 TRACKING_HZ = 125_000_000.0 SAMPLES_PER_CODE = 16_367.981 # physical RF samples/ms used by the C code CODE_FREQ_BASIS = 1.023e6 NAV_SOLVE_PERIOD_S = 10 * 0.020 POSTPONE_TOW_S = 0.180 OMEGA_EARTH = 7.2921151467e-5 WEEK_S = 604_800.0 URA_SIGMA_M = np.asarray( [ 2.4, 3.4, 4.85, 6.85, 9.65, 13.65, 24.0, 48.0, 96.0, 192.0, 384.0, 768.0, 1536.0, 3072.0, 6144.0, ], dtype=float, ) @dataclass(frozen=True) class IonoUtc: alpha: np.ndarray beta: np.ndarray DEFAULT_IONO = IonoUtc( alpha=np.asarray([1.2e-8, 1.0e-8, -5.96e-8, 0.0]), beta=np.asarray([9.0e4, 0.0, -1.31e5, 0.0]), ) @dataclass class ClockTrajectory: """Integrated oscillator trajectory, expressed in nominal clock seconds.""" time_s: np.ndarray fractional_frequency: np.ndarray local_seconds: np.ndarray nominal_hz: float initial_ticks: float = 0.0 def seconds_at(self, time_s: float | np.ndarray) -> np.ndarray: return np.interp(time_s, self.time_s, self.local_seconds) def ticks_at(self, time_s: float | np.ndarray) -> np.ndarray: return self.initial_ticks + self.nominal_hz * self.seconds_at(time_s) def time_at_ticks(self, ticks: float | np.ndarray) -> np.ndarray: local_seconds = (np.asarray(ticks) - self.initial_ticks) / self.nominal_hz return np.interp(local_seconds, self.local_seconds, self.time_s) def ppm_at(self, time_s: float | np.ndarray) -> np.ndarray: return 1.0e6 * np.interp(time_s, self.time_s, self.fractional_frequency) @dataclass(frozen=True) class Satellite: svid: int ecef_m: np.ndarray ura_index: int clock_bias_s: float clock_drift: float def clock_correction(self, time_s: float) -> float: return self.clock_bias_s + self.clock_drift * time_s @dataclass class ChannelMeasurement: satellite: Satellite satellite_position_m: np.ndarray geometric_range_m: float azimuth_deg: float elevation_deg: float ionosphere_m: float troposphere_m: float satellite_clock_s: float arrival_true_s: float raw_abs_sample: int corrected_abs_register: float corrected_abs_unwrapped: float rem_code_phase: int tracking_tick_raw: int tracking_tick_corrected: float @dataclass class WlsResult: valid: bool position_ecef_m: np.ndarray clock_bias_m: float iterations: int final_position_update_m: float final_clock_update_m: float @dataclass class EpochResult: sequence: int transmit_true_s: float reference_arrival_true_s: float snapshot_ready_true_s: float reference_tracking_tick_raw: int reference_tracking_tick: float reference_abs_sample: float reference_svid: int satellite_count: int wls: WlsResult gps_reference_estimate_s: float gps_reference_true_s: float timing_measurement_error_s: float compute_delay_s: float apb_delay_s: float apply_true_s: float raw_bias_phase_s: float def build_clock_trajectory( duration_s: float, nominal_hz: float, rng: np.random.Generator, *, base_ppm: float, temperature_ppm: float, temperature_period_s: float, random_walk_ppm_sqrt_s: float, shared_fractional: np.ndarray | None = None, grid_dt_s: float = 0.001, initial_ticks: float = 0.0, ) -> ClockTrajectory: time_s = np.arange(0.0, duration_s + 1.0 + grid_dt_s, grid_dt_s) if shared_fractional is not None: if shared_fractional.shape != time_s.shape: raise ValueError("shared clock trajectory grid mismatch") fractional = shared_fractional.copy() else: fractional = np.zeros_like(time_s) fractional += base_ppm * 1.0e-6 fractional += temperature_ppm * 1.0e-6 * np.sin( 2.0 * np.pi * time_s / temperature_period_s + 0.37 ) if random_walk_ppm_sqrt_s > 0.0: walk_step = random_walk_ppm_sqrt_s * 1.0e-6 * np.sqrt(grid_dt_s) fractional += np.cumsum(rng.normal(0.0, walk_step, time_s.size)) local_seconds = np.zeros_like(time_s) local_seconds[1:] = np.cumsum( 0.5 * ((1.0 + fractional[:-1]) + (1.0 + fractional[1:])) * np.diff(time_s) ) return ClockTrajectory( time_s=time_s, fractional_frequency=fractional, local_seconds=local_seconds, nominal_hz=nominal_hz, initial_ticks=initial_ticks, ) def lla_to_ecef(latitude_deg: float, longitude_deg: float, height_m: float) -> np.ndarray: a = 6_378_137.0 inv_f = 298.257223563 f = 1.0 / inv_f e2 = f * (2.0 - f) lat = np.deg2rad(latitude_deg) lon = np.deg2rad(longitude_deg) sin_lat = np.sin(lat) cos_lat = np.cos(lat) n = a / np.sqrt(1.0 - e2 * sin_lat * sin_lat) return np.asarray( [ (n + height_m) * cos_lat * np.cos(lon), (n + height_m) * cos_lat * np.sin(lon), (n * (1.0 - e2) + height_m) * sin_lat, ] ) def ecef_to_lla(ecef_m: np.ndarray) -> tuple[float, float, float]: x, y, z = np.asarray(ecef_m, dtype=float) a = 6_378_137.0 inv_f = 298.257223563 f = 1.0 / inv_f e2 = f * (2.0 - f) lon = np.arctan2(y, x) p = np.hypot(x, y) lat = np.arctan2(z, p * (1.0 - e2)) height = 0.0 for _ in range(12): sin_lat = np.sin(lat) n = a / np.sqrt(1.0 - e2 * sin_lat * sin_lat) height = p / max(np.cos(lat), 1.0e-15) - n lat_next = np.arctan2(z, p * (1.0 - e2 * n / (n + height))) if abs(lat_next - lat) < 1.0e-14: lat = lat_next break lat = lat_next return float(np.rad2deg(lat)), float(np.rad2deg(lon) % 360.0), float(height) def enu_basis(receiver_ecef_m: np.ndarray) -> tuple[np.ndarray, np.ndarray, np.ndarray]: lat_deg, lon_deg, _ = ecef_to_lla(receiver_ecef_m) lat = np.deg2rad(lat_deg) lon = np.deg2rad(lon_deg) east = np.asarray([-np.sin(lon), np.cos(lon), 0.0]) north = np.asarray( [-np.sin(lat) * np.cos(lon), -np.sin(lat) * np.sin(lon), np.cos(lat)] ) up = np.asarray( [np.cos(lat) * np.cos(lon), np.cos(lat) * np.sin(lon), np.sin(lat)] ) return east, north, up def azel_from_ecef( receiver_ecef_m: np.ndarray, satellite_ecef_m: np.ndarray ) -> tuple[float, float]: east, north, up = enu_basis(receiver_ecef_m) los = satellite_ecef_m - receiver_ecef_m e = float(np.dot(los, east)) n = float(np.dot(los, north)) u = float(np.dot(los, up)) horizontal = np.hypot(e, n) az = np.rad2deg(np.arctan2(e, n)) % 360.0 el = np.rad2deg(np.arctan2(u, horizontal)) return float(az), float(el) def make_satellite_constellation(receiver_ecef_m: np.ndarray) -> list[Satellite]: # Includes a deliberately weak four-satellite cold-start geometry, then # additional satellites that improve the solution as acquisition proceeds. azel = [ (15.0, 22.0), (85.0, 35.0), (170.0, 27.0), (265.0, 18.0), (320.0, 58.0), (120.0, 72.0), (220.0, 46.0), (45.0, 12.0), ] east, north, up = enu_basis(receiver_ecef_m) orbit_radius_m = 26_560_000.0 satellites: list[Satellite] = [] for index, (az_deg, el_deg) in enumerate(azel): az = np.deg2rad(az_deg) el = np.deg2rad(el_deg) direction = ( np.cos(el) * np.sin(az) * east + np.cos(el) * np.cos(az) * north + np.sin(el) * up ) rd = float(np.dot(receiver_ecef_m, direction)) discriminant = rd * rd + orbit_radius_m**2 - float( np.dot(receiver_ecef_m, receiver_ecef_m) ) distance = -rd + np.sqrt(discriminant) position = receiver_ecef_m + distance * direction satellites.append( Satellite( svid=index + 1, ecef_m=position, ura_index=0 if index < 6 else 1, clock_bias_s=(index - 3.0) * 2.7e-6, clock_drift=(index - 4.0) * 2.0e-12, ) ) return satellites def active_satellites(satellites: list[Satellite], time_s: float) -> list[Satellite]: if time_s < 20.0: active = satellites[:4] elif time_s < 50.0: active = satellites[:5] else: active = satellites[:7] # Exercise drop/re-acquisition and reference-satellite changes. if 80.0 <= time_s < 100.0: active = [sat for sat in active if sat.svid != 1] if satellites[7] not in active: active.append(satellites[7]) return active def rotate_satellite(satellite_ecef_m: np.ndarray, travel_time_s: float) -> np.ndarray: angle = OMEGA_EARTH * travel_time_s cos_angle = np.cos(angle) sin_angle = np.sin(angle) x, y, z = satellite_ecef_m return np.asarray( [cos_angle * x + sin_angle * y, -sin_angle * x + cos_angle * y, z] ) def corrected_geometric_range( receiver_ecef_m: np.ndarray, satellite_ecef_m: np.ndarray ) -> tuple[float, np.ndarray]: travel_time = np.linalg.norm(satellite_ecef_m - receiver_ecef_m) / C rotated = satellite_ecef_m.copy() for _ in range(3): rotated = rotate_satellite(satellite_ecef_m, travel_time) travel_time = np.linalg.norm(rotated - receiver_ecef_m) / C return float(travel_time * C), rotated def klobuchar_delay_m( iono: IonoUtc | None, gps_tow_s: float, latitude_deg: float, longitude_deg: float, azimuth_deg: float, elevation_deg: float, ) -> float: if iono is None or elevation_deg <= 0.0: return 0.0 elevation_sc = elevation_deg / 180.0 latitude_sc = latitude_deg / 180.0 longitude_sc = longitude_deg / 180.0 azimuth_rad = np.deg2rad(azimuth_deg) psi = 0.0137 / (elevation_sc + 0.11) - 0.022 subion_lat = np.clip(latitude_sc + psi * np.cos(azimuth_rad), -0.416, 0.416) subion_lon = longitude_sc + psi * np.sin(azimuth_rad) / np.cos(subion_lat * np.pi) geomag_lat = subion_lat + 0.064 * np.cos((subion_lon - 1.617) * np.pi) local_time = (43_200.0 * subion_lon + gps_tow_s) % 86_400.0 amplitude = iono.alpha[0] + geomag_lat * ( iono.alpha[1] + geomag_lat * (iono.alpha[2] + geomag_lat * iono.alpha[3]) ) amplitude = max(amplitude, 0.0) period = iono.beta[0] + geomag_lat * ( iono.beta[1] + geomag_lat * (iono.beta[2] + geomag_lat * iono.beta[3]) ) period = max(period, 72_000.0) phase = 2.0 * np.pi * (local_time - 50_400.0) / period obliquity = 1.0 + 16.0 * (0.53 - elevation_sc) ** 3 delay_s = 5.0e-9 if abs(phase) < 1.57: phase2 = phase * phase delay_s += amplitude * (1.0 - phase2 / 2.0 + phase2 * phase2 / 24.0) return float(obliquity * delay_s * C) def c_tropo_delay_m(sin_elevation: float, station_height_km: float) -> float: """Direct port of the active C troposphere model.""" height = station_height_km pressure = 1013.0 temperature = 293.0 humidity = 50.0 pressure_height = 0.0 temperature_height = 0.0 humidity_height = 0.0 earth_radius_km = 6378.137 b0 = 7.839257e-5 lapse = -6.5 em = -978.77 / (2.8704e6 * lapse * 1.0e-5) humidity_temp = temperature + lapse * (humidity_height - temperature_height) exponent = 7.5 * (humidity_temp - 273.15) / (237.3 + humidity_temp - 273.15) e0 = 0.0611 * humidity * 10.0**exponent sea_temp = temperature - lapse * temperature_height humidity_ref_temp = sea_temp + lapse * pressure_height e0_sea = e0 * (sea_temp / humidity_ref_temp) ** (4.0 * em) pressure_ref_temp = sea_temp + lapse * pressure_height pressure_sea = pressure * (sea_temp / pressure_ref_temp) ** em sin_elevation = max(sin_elevation, 0.0) total_delay = 0.0 dry = True ref_sea = 77.624e-6 / sea_temp top_height = 1.1385e-5 / ref_sea ref_sea *= pressure_sea ref = ref_sea * ((top_height - height) / top_height) ** 4 while True: top_range_sq = (earth_radius_km + top_height) ** 2 - ( earth_radius_km + height ) ** 2 * (1.0 - sin_elevation**2) top_range = np.sqrt(max(top_range_sq, 0.0)) - ( earth_radius_km + height ) * sin_elevation a_term = -sin_elevation / (top_height - height) b_term = -b0 * (1.0 - sin_elevation**2) / (top_height - height) rn = np.asarray([top_range ** (index + 2) for index in range(8)]) alpha = np.asarray( [ 2.0 * a_term, 2.0 * a_term**2 + 4.0 * b_term / 3.0, a_term * (a_term**2 + 3.0 * b_term), a_term**4 / 5.0 + 2.4 * a_term**2 * b_term + 1.2 * b_term**2, 2.0 * a_term * b_term * (a_term**2 + 3.0 * b_term) / 3.0, b_term**2 * (6.0 * a_term**2 + 4.0 * b_term) * 1.428571e-1, 0.0, 0.0, ] ) if b_term**2 > 1.0e-35: alpha[6] = a_term * b_term**3 / 2.0 alpha[7] = b_term**4 / 9.0 path_km = top_range + float(np.dot(alpha, rn)) total_delay += path_km * ref * 1000.0 if not dry: return float(total_delay) dry = False ref_sea = (371_900.0e-6 / sea_temp - 12.92e-6) / sea_temp top_height = 1.1385e-5 * (1255.0 / sea_temp + 0.05) / ref_sea ref = ( ref_sea * e0_sea * ((top_height - height) / top_height) ** 4 ) def measurement_weight(ura_index: int, elevation_deg: float | None) -> float: if not 0 <= ura_index < URA_SIGMA_M.size: return 0.0 variance = URA_SIGMA_M[ura_index] ** 2 if elevation_deg is not None: bounded = np.clip(elevation_deg, 10.0, 90.0) elevation_sigma = 3.0 / np.sin(np.deg2rad(bounded)) variance += elevation_sigma**2 return float(1.0 / variance) def signed_u32_delta(value: int, reference: int) -> int: difference = (int(value) - int(reference)) & 0xFFFF_FFFF return difference - 0x1_0000_0000 if difference & 0x8000_0000 else difference def is_u32_after(value: int, reference: int) -> bool: return signed_u32_delta(value, reference) > 0 def unwrap_corrected_samples(measurements: list[ChannelMeasurement]) -> None: newest = measurements[0].raw_abs_sample for measurement in measurements[1:]: if is_u32_after(measurement.raw_abs_sample, newest): newest = measurement.raw_abs_sample for measurement in measurements: correction = measurement.raw_abs_sample - measurement.corrected_abs_register measurement.corrected_abs_unwrapped = ( float(newest) + signed_u32_delta(measurement.raw_abs_sample, newest) - correction ) def weighted_least_squares_like_c( satellite_positions_m: np.ndarray, observations_m: np.ndarray, ura_indices: np.ndarray, gps_tow_s: float, iono: IonoUtc | None, previous_solution: np.ndarray | None, *, max_iterations: int = 100, ) -> WlsResult: count = satellite_positions_m.shape[0] if count < 4: return WlsResult(False, np.zeros(3), np.nan, 0, np.nan, np.nan) seeded = previous_solution is not None and np.all(np.isfinite(previous_solution)) state = previous_solution.copy() if seeded else np.zeros(4) final_position_update = np.inf final_clock_update = np.inf for iteration in range(max_iterations): design = np.zeros((count, 4)) residual = np.zeros(count) weights = np.zeros(count) for index in range(count): if iteration == 0 and not seeded: rotated = satellite_positions_m[index] ionosphere = 0.0 troposphere = 2.0 elevation = None else: travel_time = np.linalg.norm(satellite_positions_m[index] - state[:3]) / C rotated = rotate_satellite(satellite_positions_m[index], travel_time) azimuth, elevation_value = azel_from_ecef(state[:3], rotated) elevation = elevation_value latitude, longitude, height = ecef_to_lla(state[:3]) ionosphere = klobuchar_delay_m( iono, gps_tow_s, latitude, longitude, azimuth, elevation_value, ) troposphere = 0.0 if elevation_value > 0.0 and -1000.0 <= height <= 15_000.0: troposphere = c_tropo_delay_m( np.sin(np.deg2rad(elevation_value)), height / 1000.0 ) if not np.isfinite(troposphere) or troposphere < 0.0: troposphere = 0.0 difference = rotated - state[:3] geometric = float(np.linalg.norm(difference)) residual[index] = ( observations_m[index] - geometric - state[3] - troposphere - ionosphere ) denominator = geometric if geometric > 1.0 else observations_m[index] design[index, :3] = -difference / denominator design[index, 3] = 1.0 weights[index] = measurement_weight(int(ura_indices[index]), elevation) if not np.all(np.isfinite(weights)) or np.any(weights <= 0.0): return WlsResult(False, state[:3], state[3], iteration, np.nan, np.nan) weights /= np.max(weights) # mirrors least_squares_normalize_weights() normal = design.T @ (weights[:, None] * design) rhs = design.T @ (weights * residual) try: update = np.linalg.solve(normal, rhs) except np.linalg.LinAlgError: return WlsResult(False, state[:3], state[3], iteration, np.nan, np.nan) state += update final_position_update = float(np.linalg.norm(update[:3])) final_clock_update = float(update[3]) # normVect(xVec) in the C implementation reads only x/y/z. if final_position_update < 1.0e-4: return WlsResult( True, state[:3].copy(), float(state[3]), iteration + 1, final_position_update, final_clock_update, ) return WlsResult( True, state[:3].copy(), float(state[3]), max_iterations, final_position_update, final_clock_update, ) class ReceiverEpochSimulator: def __init__( self, receiver_ecef_m: np.ndarray, satellites: list[Satellite], rf_clock: ClockTrajectory, tracking_clock: ClockTrajectory, rng: np.random.Generator, *, gps_tow_start_s: float = 345_600.0, iono: IonoUtc | None = DEFAULT_IONO, ) -> None: self.receiver_ecef_m = np.asarray(receiver_ecef_m, dtype=float) self.satellites = satellites self.rf_clock = rf_clock self.tracking_clock = tracking_clock self.rng = rng self.gps_tow_start_s = gps_tow_start_s self.iono = iono self.last_solution: np.ndarray | None = None def _measure_channel( self, satellite: Satellite, transmit_true_s: float ) -> ChannelMeasurement: geometric, rotated = corrected_geometric_range( self.receiver_ecef_m, satellite.ecef_m ) azimuth, elevation = azel_from_ecef(self.receiver_ecef_m, rotated) latitude, longitude, height = ecef_to_lla(self.receiver_ecef_m) tow = (self.gps_tow_start_s + transmit_true_s + POSTPONE_TOW_S) % WEEK_S ionosphere = klobuchar_delay_m( self.iono, tow, latitude, longitude, azimuth, elevation ) troposphere = c_tropo_delay_m( np.sin(np.deg2rad(max(elevation, 0.0))), height / 1000.0 ) satellite_clock = satellite.clock_correction(transmit_true_s) sigma = np.sqrt( URA_SIGMA_M[satellite.ura_index] ** 2 + (3.0 / np.sin(np.deg2rad(max(elevation, 10.0)))) ** 2 ) # This noise represents DLL/code multipath and unmodelled atmosphere. path_error_m = self.rng.normal(0.0, sigma) signal_path_m = geometric + ionosphere + troposphere + path_error_m arrival_true_s = transmit_true_s + signal_path_m / C - satellite_clock continuous_abs = float(self.rf_clock.ticks_at(arrival_true_s)) raw_unwrapped = int(np.ceil(continuous_abs)) fractional_samples = raw_unwrapped - continuous_abs raw_abs = raw_unwrapped & 0xFFFF_FFFF rem = int(np.rint(fractional_samples * CODE_FREQ_BASIS)) rem = int(np.clip(rem, 0, (1 << 26) - 1)) corrected_register = float(raw_abs) - rem / CODE_FREQ_BASIS # ABS_SAMPLE is the RF sample edge after the fractional code boundary. # Capture the 125 MHz timestamp at that raw edge, then apply the same # REM_CODE_PHASE correction to refer both clocks to the code boundary. raw_event_true_s = float(self.rf_clock.time_at_ticks(raw_unwrapped)) tracking_tick_continuous = float(self.tracking_clock.ticks_at(raw_event_true_s)) tracking_tick_raw = int(np.floor(tracking_tick_continuous)) # The software correction refers to the code boundary before the raw # RF edge. The mailbox accepts an integer 125 MHz epoch counter, so # mirror the C implementation's nearest-tick rounding. capture_correction_ticks = round( (rem / CODE_FREQ_BASIS) * (TRACKING_HZ / RF_HZ) ) tracking_tick_corrected = tracking_tick_raw - capture_correction_ticks return ChannelMeasurement( satellite=satellite, satellite_position_m=satellite.ecef_m.copy(), geometric_range_m=geometric, azimuth_deg=azimuth, elevation_deg=elevation, ionosphere_m=ionosphere, troposphere_m=troposphere, satellite_clock_s=satellite_clock, arrival_true_s=arrival_true_s, raw_abs_sample=raw_abs, corrected_abs_register=corrected_register, corrected_abs_unwrapped=np.nan, rem_code_phase=rem, tracking_tick_raw=tracking_tick_raw, tracking_tick_corrected=tracking_tick_corrected, ) def simulate_epoch(self, sequence: int) -> EpochResult | None: transmit_true_s = sequence * NAV_SOLVE_PERIOD_S active = active_satellites(self.satellites, transmit_true_s) if len(active) < 4: return None measurements = [ self._measure_channel(satellite, transmit_true_s) for satellite in active ] unwrap_corrected_samples(measurements) corrected_abs = np.asarray( [measurement.corrected_abs_unwrapped for measurement in measurements] ) reference_index = int(np.argmin(corrected_abs)) # All channels in this synthetic epoch share one transmit time. Mirror # firmware's wrap-safe corrected RF-sample difference. The fractional # code residue removes the 61 ns integer-sample quantization; the # separately captured 125 MHz counter is used only for PPS epoch mapping. observations_m = ( corrected_abs - corrected_abs[reference_index] ) * (C / RF_HZ) observations_m += C * np.asarray( [measurement.satellite_clock_s for measurement in measurements] ) satellite_positions = np.asarray( [measurement.satellite_position_m for measurement in measurements] ) ura_indices = np.asarray( [measurement.satellite.ura_index for measurement in measurements] ) gps_tow = ( self.gps_tow_start_s + transmit_true_s + POSTPONE_TOW_S ) % WEEK_S wls = weighted_least_squares_like_c( satellite_positions, observations_m, ura_indices, gps_tow, self.iono, self.last_solution, ) if wls.valid: self.last_solution = np.r_[wls.position_ecef_m, wls.clock_bias_m] reference = measurements[reference_index] gps_reference_estimate = ( self.gps_tow_start_s + transmit_true_s - wls.clock_bias_m / C ) gps_reference_true = self.gps_tow_start_s + reference.arrival_true_s snapshot_ready = max(measurement.arrival_true_s for measurement in measurements) compute_delay = max(0.001, self.rng.normal(0.010, 0.003)) if 35.0 <= transmit_true_s < 36.0 or 110.0 <= transmit_true_s < 110.6: compute_delay += self.rng.uniform(0.040, 0.090) apb_delay = max(20.0e-6, self.rng.normal(180.0e-6, 70.0e-6)) apply_time = snapshot_ready + compute_delay + apb_delay return EpochResult( sequence=sequence, transmit_true_s=transmit_true_s, reference_arrival_true_s=reference.arrival_true_s, snapshot_ready_true_s=snapshot_ready, reference_tracking_tick_raw=reference.tracking_tick_raw, reference_tracking_tick=reference.tracking_tick_corrected, reference_abs_sample=reference.corrected_abs_unwrapped, reference_svid=reference.satellite.svid, satellite_count=len(measurements), wls=wls, gps_reference_estimate_s=gps_reference_estimate, gps_reference_true_s=gps_reference_true, timing_measurement_error_s=gps_reference_estimate - gps_reference_true, compute_delay_s=compute_delay, apb_delay_s=apb_delay, apply_true_s=apply_time, raw_bias_phase_s=wls.clock_bias_m / C, ) def simulate(self, duration_s: float) -> list[EpochResult]: epoch_count = int(np.floor(duration_s / NAV_SOLVE_PERIOD_S)) results: list[EpochResult] = [] for sequence in range(epoch_count): epoch = self.simulate_epoch(sequence) if epoch is not None and epoch.wls.valid: results.append(epoch) return results def as_array(epochs: Iterable[EpochResult], attribute: str) -> np.ndarray: return np.asarray([getattr(epoch, attribute) for epoch in epochs])