754 lines
26 KiB
Python
754 lines
26 KiB
Python
"""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 * ((abs_i - min(abs)) / SAMPLES_PER_CODE + STARTOFFSET) / 1000
|
|
* 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_368_000.0
|
|
TRACKING_HZ = 125_000_000.0
|
|
SAMPLES_PER_CODE = 16_368.0 # samples per millisecond in the C code
|
|
CODE_FREQ_BASIS = 1.023e6
|
|
START_OFFSET_MS = 68.802
|
|
START_OFFSET_S = START_OFFSET_MS / 1000.0
|
|
NAV_SOLVE_PERIOD_S = 10 * 0.020
|
|
POSTPONE_TOW_S = 0.160
|
|
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: 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. Apply the same time displacement to the tracking timestamp.
|
|
tracking_tick_corrected = tracking_tick_raw - (
|
|
rem / CODE_FREQ_BASIS
|
|
) * (TRACKING_HZ / RF_HZ)
|
|
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]
|
|
)
|
|
ps_tvl_ms = corrected_abs / SAMPLES_PER_CODE
|
|
reference_index = int(np.argmin(ps_tvl_ms))
|
|
precise_tx_ms = ps_tvl_ms - ps_tvl_ms[reference_index] + START_OFFSET_MS
|
|
observations_m = precise_tx_ms * (C / 1000.0)
|
|
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
|
|
+ START_OFFSET_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=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])
|