From f0f94b31fecd20ffa33d849ff09c6573e9a80ae3 Mon Sep 17 00:00:00 2001 From: Ryan Kingsbury Date: Tue, 21 Jun 2016 17:32:09 -0700 Subject: [PATCH] Rebase updates mostly related to short_set and warm_start --- peregrine/acquisition.py | 10 ++++++---- peregrine/short_set.py | 34 +++++++++++++++++----------------- peregrine/warm_start.py | 7 +++++-- 3 files changed, 28 insertions(+), 23 deletions(-) diff --git a/peregrine/acquisition.py b/peregrine/acquisition.py index 9d4cc56..72cc9e8 100644 --- a/peregrine/acquisition.py +++ b/peregrine/acquisition.py @@ -54,14 +54,16 @@ class Acquisition: Array of samples to use for acquisition. Can be `None` but in this case `init_samples` *must* be called with an array of samples before any other acquisition functions are used. - sampling_freq : float, optional + sampling_freq : float The sampling frequency of the samples in Hz. - IF : float, optional + IF : float The receiver intermediate frequency used when capturing the samples. - samples_per_code : float, optional + samples_per_code : float The number of samples corresponding to one code length. - code_length : int, optional + code_length : int The number of chips in the chipping code. + n_codes_integrate : int, optional + The number of codes to integrate. offsets : int, optional Offsets, in units of code length (1ms), to use when performing long integrations to avoid clobbering by nav bit edges. diff --git a/peregrine/short_set.py b/peregrine/short_set.py index bfa9f2a..eb22e5f 100644 --- a/peregrine/short_set.py +++ b/peregrine/short_set.py @@ -48,12 +48,12 @@ def resolve_ms_integers(obs_pr, pred_pr, prn_ref, disp = True): print "Resolving millisecond integers:" for prn, pr in obs_pr.iteritems(): - pr_int_est = (pred_pr[prn] - pr) / gps.code_wavelength + pr_int_est = (pred_pr[prn] - pr) / gps.l1ca_code_wavelength pr_int = round(pr_int_est) if abs(pr_int - pr_int_est) > 0.15: logger.warn("Pseudorange integer for PRN %2d is %.4f" % ( prn + 1, pr_int_est) + ", which isn't very close to an integer.") - pr += pr_int * gps.code_wavelength + pr += pr_int * gps.l1ca_code_wavelength obs_pr[prn] = pr if disp: print ("PRN %2d: pred pseudorange = %9.3f km, obs = %9.3f, " + \ @@ -78,9 +78,9 @@ def fill_remainder(n_ms): return [k for k,v in itertools.groupby(sorted(hs))] def long_correlation(signal, ca_code, code_phase, doppler, settings, plot=False, coherent = 0, nav_bit_hypoth = None): - from swiftnav.correlate import track_correlate_ + from swiftnav.correlate import track_correlate code_freq_shift = (doppler / gps.l1) * gps.chip_rate - samples_per_chip = settings.samplingFreq / (gps.chip_rate + code_freq_shift) + samples_per_chip = settings.freq_profile['sampling_freq'] / (gps.chip_rate + code_freq_shift) samples_per_code = samples_per_chip * gps.chips_per_code numSamplesToSkip = round(code_phase * samples_per_chip) remCodePhase = (1.0 * numSamplesToSkip / samples_per_chip) - code_phase @@ -94,12 +94,14 @@ def long_correlation(signal, ca_code, code_phase, doppler, settings, plot=False, costas_q = 0.0 for loopCnt in range(n_ms): rawSignal = signal[numSamplesToSkip:]#[:blksize_] - E, P, L, blksize, remCodePhase, remCarrPhase = track_correlate_( + E, P, L, blksize, remCodePhase, remCarrPhase = track_correlate( rawSignal, + 1023, # Chips to correlate code_freq_shift + gps.chip_rate, remCodePhase, - doppler + settings.IF, - remCarrPhase, ca_code, settings.samplingFreq) + doppler + settings.freq_profile['GPS_L1_IF'], + remCarrPhase, ca_code, settings.freq_profile['sampling_freq'], + gps.L1CA) I_E = E.real Q_E = E.imag @@ -149,12 +151,10 @@ def refine_ob(signal, acq_result, settings, print_results = True, return_sweeps # TODO: Fit code phase results for better resolution from peregrine.include.generateCAcode import caCodes from scipy import optimize as opt - samples_per_chip = settings.samplingFreq / gps.chip_rate + samples_per_chip = settings.freq_profile['sampling_freq'] / gps.chip_rate samples_per_code = samples_per_chip * gps.chips_per_code # Get a vector with the C/A code sampled 1x/chip ca_code = caCodes[acq_result.prn] - # Add wrapping to either end to be able to do early/late - ca_code = np.concatenate(([ca_code[1022]],ca_code,[ca_code[0]])) dopp_offset_search = 100 # Hz away from acquisition code_offsets = np.arange(-1,1, 1.0 / 16 / 2) @@ -296,14 +296,14 @@ def refine_obs(signal, acq_results, settings, return obs_cp, obs_dopp -def predict_observables(prior_traj, prior_datetime, prns, ephem, window): +def predict_observables(prior_traj, prior_datetime, prns, ephem, window, settings): from datetime import timedelta from numpy.linalg import norm from numpy import dot """Given a list of PRNs, a set of ephemerides, a nominal capture time (datetime) and a and a time window (seconds), compute the ranges and dopplers for each satellite at 1ms shifts.""" - timeres = 50 * gps.code_period # Might be important to keep this an integer number of code periods + timeres = 50 * settings.code_period # Might be important to keep this an integer number of code periods t0 = prior_datetime - timedelta(seconds=window / 2.0) ranges = {} dopplers = {} @@ -360,11 +360,11 @@ def minimize_doppler_error(obs_dopp, times, pred_dopp, plot = False): def plot_expected_vs_measured(acqed_prns, prn_ref, obs_pr, obs_dopp, prior_traj, t_better, - ephem): + ephem, settings): import matplotlib.pyplot as plt # Compute predicted observables around this new estimate of capture time - pred_ranges, pred_dopplers, times = predict_observables(prior_traj, t_better, acqed_prns, ephem, 20) + pred_ranges, pred_dopplers, times = predict_observables(prior_traj, t_better, acqed_prns, ephem, 20, settings) pred_pr = pseudoranges_from_ranges(pred_ranges, prn_ref) ax = plt.figure(figsize=(12,6)).gca() @@ -660,7 +660,7 @@ def postprocess_short_samples(signal, prior_trajectory, t_prior, settings, # Improve the time part of the prior estimate by minimizing doppler residuals pred_ranges, pred_dopplers, times = predict_observables(prior_traj, t_prior, acqed_prns, ephem, - 30) + 30, settings) i, t_better = minimize_doppler_error(obs_dopp, times, pred_dopplers, plot = plot) @@ -671,7 +671,7 @@ def postprocess_short_samples(signal, prior_trajectory, t_prior, settings, print "By minimizing doppler residuals, adjusted the prior time and position by %.6s seconds, %.3f km" % ( delta_t, delta_r/ 1e3) pred_ranges, pred_dopplers, times = predict_observables( - prior_traj, t_better, acqed_prns, ephem, 1e-9) + prior_traj, t_better, acqed_prns, ephem, 1e-9, settings) pred_pr_t_better = {prn: pred_ranges[prn][0] for prn in acqed_prns} @@ -681,7 +681,7 @@ def postprocess_short_samples(signal, prior_trajectory, t_prior, settings, if plot: plot_expected_vs_measured(acqed_prns, prn_ref, obs_pr, obs_dopp, - prior_traj, t_better, ephem) + prior_traj, t_better, ephem, settings) # Perform PVT navigation solution r_sol, t_sol, los, tot, residuals = pt_solve(r_better, t_better, obs_pr, diff --git a/peregrine/warm_start.py b/peregrine/warm_start.py index be2c005..2c2accc 100755 --- a/peregrine/warm_start.py +++ b/peregrine/warm_start.py @@ -91,9 +91,12 @@ def warm_start(signal, t_prior, r_prior, v_prior, ephem, settings, samplingFreq = settings.freq_profile['sampling_freq'] - a = acquisition.Acquisition(signal, samplingFreq, + a = acquisition.Acquisition(gps.L1CA, + signal, + samplingFreq, settings.freq_profile['GPS_L1_IF'], samplingFreq * settings.code_period, + settings.code_length, n_codes_integrate=n_codes_integrate, wisdom_file = wiz_file) # Attempt to acquire both the sats we predict are visible @@ -102,7 +105,7 @@ def warm_start(signal, t_prior, r_prior, v_prior, ephem, settings, prns = pred + notup, doppler_priors = pred_dopp + [0 for p in notup], doppler_search = settings.rxFreqTol * gps.l1, - show_progress = True, multi = True) + progress_bar_output='stderr', multi = True) nacq_results = acq_results[len(pred):] acq_results = acq_results[:len(pred)]