def correct_and_pos_fix(processed_measurements: List[GNSSMeasurement],
dog: AstroDog):
# pos fix needs more than 5 processed_measurements
pos_fix = calc_pos_fix(processed_measurements)
if len(pos_fix) == 0:
return [], []
est_pos = pos_fix[0][:3]
corrected = correct_measurements(processed_measurements, est_pos, dog)
return calc_pos_fix(corrected), corrected
def process_ublox_msg(self, ublox_msg, ublox_mono_time: int):
if ublox_msg.which == 'measurementReport':
report = ublox_msg.measurementReport
new_meas = read_raw_ublox(report)
measurements = process_measurements(new_meas, self.astro_dog)
pos_fix = calc_pos_fix(measurements, min_measurements=4)
# To get a position fix a minimum of 5 measurements are needed.
# Each report can contain less and some measurements can't be processed.
corrected_measurements = []
if len(pos_fix) > 0 and linalg.norm(pos_fix[1]) < 100:
corrected_measurements = correct_measurements(
measurements, pos_fix[0][:3], self.astro_dog)
t = ublox_mono_time * 1e-9
self.update_localizer(pos_fix, t, corrected_measurements)
localizer_valid = self.localizer_valid(t)
ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist()
ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist()
pos_std = float(np.linalg.norm(self.gnss_kf.P[GStates.ECEF_POS]))
vel_std = float(
np.linalg.norm(self.gnss_kf.P[GStates.ECEF_VELOCITY]))
bearing_deg, bearing_std = get_bearing_from_gnss(
ecef_pos, ecef_vel, vel_std)
meas_msgs = [
create_measurement_msg(m) for m in corrected_measurements
]
dat = messaging.new_message("gnssMeasurements")
measurement_msg = log.GnssMeasurements.Measurement.new_message
dat.gnssMeasurements = {
"positionECEF":
measurement_msg(value=ecef_pos,
std=pos_std,
valid=localizer_valid),
"velocityECEF":
measurement_msg(value=ecef_vel,
std=vel_std,
valid=localizer_valid),
"bearingDeg":
measurement_msg(value=[bearing_deg],
std=bearing_std,
valid=localizer_valid),
"ubloxMonoTime":
ublox_mono_time,
"correctedMeasurements":
meas_msgs
}
return dat
elif ublox_msg.which == 'ephemeris':
ephem = convert_ublox_ephem(ublox_msg.ephemeris)
self.astro_dog.add_ephem(ephem, self.astro_dog.orbits)
def test_station_position(self):
print(
'WARNING THIS TAKE CAN TAKE A VERY LONG TIME THE FIRST RUN TO DOWNLOAD'
)
dog = AstroDog()
# Building this cache takes forever just copy it from repo
cache_directory = '/tmp/gnss/cors_coord/'
try:
os.mkdir('/tmp/gnss/')
except OSError:
pass
try:
os.mkdir(cache_directory)
except OSError:
pass
examples_directory = os.path.join(
os.path.dirname(os.path.abspath(__file__)), '../examples')
copyfile(os.path.join(examples_directory, 'cors_station_positions'),
os.path.join(cache_directory, 'cors_station_positions'))
station_name = 'sc01'
time = GPSTime.from_datetime(datetime(2020, 1, 11))
slac_rinex_obs_file = download_cors_station(time, station_name,
dog.cache_dir)
obs_data = RINEXFile(slac_rinex_obs_file)
sc01_exact_position = get_station_position('sc01')
rinex_meas_grouped = raw.read_rinex_obs(obs_data)
rinex_corr_grouped = []
for meas in tqdm(rinex_meas_grouped):
proc = raw.process_measurements(meas, dog=dog)
corr = raw.correct_measurements(meas, sc01_exact_position, dog=dog)
rinex_corr_grouped.append(corr)
# Using laika's WLS solver we can now calculate position
# fixes for every epoch (every 30s) over 24h.
ests = []
for corr in tqdm(rinex_corr_grouped[:]):
fix, _ = raw.calc_pos_fix(corr)
ests.append(fix)
ests = np.array(ests)
mean_fix = np.mean(ests[:, :3], axis=0)
np.testing.assert_allclose(mean_fix,
sc01_exact_position,
rtol=0,
atol=1)
def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False):
if ublox_msg.which == 'measurementReport':
t = ublox_mono_time * 1e-9
report = ublox_msg.measurementReport
if report.gpsWeek > 0:
latest_msg_t = GPSTime(report.gpsWeek, report.rcvTow)
self.fetch_orbits(latest_msg_t + SECS_IN_MIN, block)
new_meas = read_raw_ublox(report)
processed_measurements = process_measurements(new_meas, self.astro_dog)
if self.last_pos_fix_t is None or abs(self.last_pos_fix_t - t) >= 2:
min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in processed_measurements) else 4
pos_fix, pos_fix_residual = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements)
if len(pos_fix) > 0:
self.last_pos_fix = pos_fix[:3]
self.last_pos_residual = pos_fix_residual
self.last_pos_fix_t = t
corrected_measurements = correct_measurements(processed_measurements, self.last_pos_fix, self.astro_dog) if self.last_pos_fix_t is not None else []
self.update_localizer(self.last_pos_fix, t, corrected_measurements)
kf_valid = all(self.kf_valid(t))
ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist()
ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist()
pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal())).tolist()
vel_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_VELOCITY].diagonal())).tolist()
meas_msgs = [create_measurement_msg(m) for m in corrected_measurements]
dat = messaging.new_message("gnssMeasurements")
measurement_msg = log.LiveLocationKalman.Measurement.new_message
dat.gnssMeasurements = {
"gpsWeek": report.gpsWeek,
"gpsTimeOfWeek": report.rcvTow,
"positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=kf_valid),
"velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=kf_valid),
"positionFixECEF": measurement_msg(value=self.last_pos_fix, std=self.last_pos_residual, valid=self.last_pos_fix_t == t),
"ubloxMonoTime": ublox_mono_time,
"correctedMeasurements": meas_msgs
}
return dat
elif ublox_msg.which == 'ephemeris':
ephem = convert_ublox_ephem(ublox_msg.ephemeris)
self.astro_dog.add_navs({ephem.prn: [ephem]})
self.cache_ephemeris(t=ephem.epoch)
def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False):
if ublox_msg.which == 'measurementReport':
t = ublox_mono_time * 1e-9
report = ublox_msg.measurementReport
if report.gpsWeek > 0:
self.got_first_ublox_msg = True
latest_msg_t = GPSTime(report.gpsWeek, report.rcvTow)
if self.auto_fetch_orbits:
self.fetch_orbits(latest_msg_t, block)
new_meas = read_raw_ublox(report)
# Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites
new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7]
processed_measurements = process_measurements(new_meas, self.astro_dog)
est_pos = self.get_est_pos(t, processed_measurements)
corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog) if len(est_pos) > 0 else []
self.update_localizer(est_pos, t, corrected_measurements)
kf_valid = all(self.kf_valid(t))
ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist()
ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist()
pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal())).tolist()
vel_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_VELOCITY].diagonal())).tolist()
meas_msgs = [create_measurement_msg(m) for m in corrected_measurements]
dat = messaging.new_message("gnssMeasurements")
measurement_msg = log.LiveLocationKalman.Measurement.new_message
dat.gnssMeasurements = {
"gpsWeek": report.gpsWeek,
"gpsTimeOfWeek": report.rcvTow,
"positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=kf_valid),
"velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=kf_valid),
"positionFixECEF": measurement_msg(value=self.last_pos_fix, std=self.last_pos_residual, valid=self.last_pos_fix_t == t),
"ubloxMonoTime": ublox_mono_time,
"correctedMeasurements": meas_msgs
}
return dat
elif ublox_msg.which == 'ephemeris':
ephem = convert_ublox_ephem(ublox_msg.ephemeris)
self.astro_dog.add_navs({ephem.prn: [ephem]})
self.cache_ephemeris(t=ephem.epoch)
def handle_ublox_gnss(self, log, current_time):
if hasattr(log.ubloxGnss, 'measurementReport'):
self.raw_gnss_counter += 1
if True or self.raw_gnss_counter % 3 == 0:
processed_raw = gnss.process_measurements(gnss.read_raw_ublox(
log.ubloxGnss.measurementReport),
dog=self.dog)
corrected_raw = gnss.correct_measurements(processed_raw,
self.kf.x[:3],
dog=self.dog)
corrected_raw = np.array([c.as_array()
for c in corrected_raw]).reshape(
(-1, 14))
self.update_kalman(current_time,
ObservationKind.PSEUDORANGE_GPS,
corrected_raw)
self.update_kalman(current_time,
ObservationKind.PSEUDORANGE_RATE_GPS,
corrected_raw)
def run_station_position(self, length):
dog = AstroDog()
# Building this cache takes forever just copy it from repo
cache_directory = '/tmp/gnss/cors_coord/'
os.makedirs('/tmp/gnss/', exist_ok=True)
os.makedirs(cache_directory, exist_ok=True)
examples_directory = os.path.join(
os.path.dirname(os.path.abspath(__file__)), '../examples')
copyfile(os.path.join(examples_directory, 'cors_station_positions'),
os.path.join(cache_directory, 'cors_station_positions'))
station_name = 'sc01'
time = GPSTime.from_datetime(datetime(2020, 1, 11))
slac_rinex_obs_file = download_cors_station(time, station_name,
dog.cache_dir)
obs_data = RINEXFile(slac_rinex_obs_file)
sc01_exact_position = get_station_position('sc01')
rinex_meas_grouped = raw.read_rinex_obs(obs_data)
# Select small sample out of ~2800 to reduce computation time
rinex_meas_grouped = rinex_meas_grouped[:length]
rinex_corr_grouped = []
for meas in tqdm(rinex_meas_grouped):
proc = raw.process_measurements(meas, dog=dog)
corr = raw.correct_measurements(proc, sc01_exact_position, dog=dog)
rinex_corr_grouped.append(corr)
# Using laika's WLS solver we can now calculate position
# fixes for every epoch (every 30s) over 24h.
ests = []
for corr in tqdm(rinex_corr_grouped):
ret = raw.calc_pos_fix(corr)
if len(ret) > 0:
fix, _ = ret
ests.append(fix)
ests = np.array(ests)
mean_fix = np.mean(ests[:, :3], axis=0)
np.testing.assert_allclose(mean_fix,
sc01_exact_position,
rtol=0,
atol=1)
def test_get_fix(self):
dog = AstroDog()
position_fix_found = 0
count_processed_measurements = 0
count_corrected_measurements = 0
position_fix_found_after_correcting = 0
pos_ests = []
for measurements in self.gnss_measurements[:self.
NUM_TEST_PROCESS_MEAS]:
processed_meas = process_measurements(measurements, dog)
count_processed_measurements += len(processed_meas)
pos_fix = calc_pos_fix(processed_meas)
if len(pos_fix) > 0 and all(pos_fix[0] != 0):
position_fix_found += 1
corrected_meas = correct_measurements(processed_meas,
pos_fix[0][:3], dog)
count_corrected_measurements += len(corrected_meas)
pos_fix = calc_pos_fix(corrected_meas)
if len(pos_fix) > 0 and all(pos_fix[0] != 0):
pos_ests.append(pos_fix[0])
position_fix_found_after_correcting += 1
mean_fix = np.mean(np.array(pos_ests)[:, :3], axis=0)
np.testing.assert_allclose(
mean_fix, [-2452306.662377, -4778343.136806, 3428550.090557],
rtol=0,
atol=1)
# Note that can happen that there are less corrected measurements compared to processed when they are invalid.
# However, not for the current segment
self.assertEqual(position_fix_found, self.NUM_TEST_PROCESS_MEAS)
self.assertEqual(position_fix_found_after_correcting,
self.NUM_TEST_PROCESS_MEAS)
self.assertEqual(count_processed_measurements, 69)
self.assertEqual(count_corrected_measurements, 69)
def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False):
if self.is_good_report(gnss_msg):
week, tow, new_meas = self.read_report(gnss_msg)
t = gnss_mono_time * 1e-9
if week > 0:
self.got_first_gnss_msg = True
latest_msg_t = GPSTime(week, tow)
if self.auto_fetch_orbits:
self.fetch_orbits(latest_msg_t, block)
# Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites
new_meas = [
m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7
]
processed_measurements = process_measurements(
new_meas, self.astro_dog)
est_pos = self.get_est_pos(t, processed_measurements)
corrected_measurements = correct_measurements(
processed_measurements, est_pos,
self.astro_dog) if len(est_pos) > 0 else []
if gnss_mono_time % 10 == 0:
cloudlog.debug(
f"Measurements Incoming/Processed/Corrected: {len(new_meas), len(processed_measurements), len(corrected_measurements)}"
)
self.update_localizer(est_pos, t, corrected_measurements)
kf_valid = all(self.kf_valid(t))
ecef_pos = self.gnss_kf.x[GStates.ECEF_POS]
ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY]
p = self.gnss_kf.P.diagonal()
pos_std = np.sqrt(p[GStates.ECEF_POS])
vel_std = np.sqrt(p[GStates.ECEF_VELOCITY])
meas_msgs = [
create_measurement_msg(m) for m in corrected_measurements
]
dat = messaging.new_message("gnssMeasurements")
measurement_msg = log.LiveLocationKalman.Measurement.new_message
dat.gnssMeasurements = {
"gpsWeek":
week,
"gpsTimeOfWeek":
tow,
"positionECEF":
measurement_msg(value=ecef_pos.tolist(),
std=pos_std.tolist(),
valid=kf_valid),
"velocityECEF":
measurement_msg(value=ecef_vel.tolist(),
std=vel_std.tolist(),
valid=kf_valid),
"positionFixECEF":
measurement_msg(value=self.last_pos_fix,
std=self.last_pos_residual,
valid=self.last_pos_fix_t == t),
"ubloxMonoTime":
gnss_mono_time,
"correctedMeasurements":
meas_msgs
}
return dat
# TODO this only works on GLONASS, qcom needs live ephemeris parsing too
elif gnss_msg.which == 'ephemeris':
ephem = convert_ublox_ephem(gnss_msg.ephemeris)
self.astro_dog.add_navs({ephem.prn: [ephem]})
self.cache_ephemeris(t=ephem.epoch)