From cfca8b53e9792d2b44080efdc8fbe6036cd4fb2a Mon Sep 17 00:00:00 2001 From: Fergus Noble Date: Tue, 17 Nov 2015 17:04:38 -0800 Subject: [PATCH 1/2] TRACKING: Add option to use receive GPS time for pseudorange calculation Also clean up this function and add doxygen documentation. This can be used to fix the issue that the pseudorange rate was not proportional to the Doppler and fixes the large time varying receiver clock bias implied by the previous reference sat method. --- include/libswiftnav/track.h | 6 +- src/track.c | 116 +++++++++++++++++++++++++++--------- 2 files changed, 93 insertions(+), 29 deletions(-) diff --git a/include/libswiftnav/track.h b/include/libswiftnav/track.h index eae5a80d..5f00fd00 100644 --- a/include/libswiftnav/track.h +++ b/include/libswiftnav/track.h @@ -233,10 +233,12 @@ float cn0_est(cn0_est_state_t *s, float I, float Q); void calc_navigation_measurement(u8 n_channels, channel_measurement_t meas[], navigation_measurement_t nav_meas[], - double nav_time, ephemeris_t ephemerides[]); + double nav_time_tc, gps_time_t *nav_time, + ephemeris_t ephemerides[]); void calc_navigation_measurement_(u8 n_channels, channel_measurement_t* meas[], navigation_measurement_t* nav_meas[], - double nav_time, ephemeris_t* ephemerides[]); + double nav_time_tc, gps_time_t *nav_time, + ephemeris_t* ephemerides[]); int nav_meas_cmp(const void *a, const void *b); u8 tdcp_doppler(u8 n_new, navigation_measurement_t *m_new, diff --git a/src/track.c b/src/track.c index 8bc57765..3e73e392 100644 --- a/src/track.c +++ b/src/track.c @@ -745,8 +745,25 @@ float cn0_est(cn0_est_state_t *s, float I, float Q) return s->log_bw - 10.f*log10f(s->nsr); } +/** Calculate observations from tracking channel measurements. + * + * This function takes flat arrays, for a version taking a arrays of pointers + * see calc_navigation_measurement_(). + * + * \param n_channels Number of tracking channel measurements + * \param meas Array of tracking channel measurements, length `n_channels` + * \param nav_meas Output array for the observations, length `n_channels` + * \param nav_time_rx Receiver time at which to calculate the position solution + * in seconds + * \param nav_time Pointer to GPS time at which to calculate the position + * solution. Can be `NULL` in which case one of the + * pseudoranges is chosen as a reference and set to a nominal + * range, implying a certain receiver clock error + * \param ephemerides Array of ephemerides + */ void calc_navigation_measurement(u8 n_channels, channel_measurement_t meas[], - navigation_measurement_t nav_meas[], double nav_time, + navigation_measurement_t nav_meas[], + double nav_time_rx, gps_time_t *nav_time, ephemeris_t ephemerides[]) { channel_measurement_t* meas_ptrs[n_channels]; @@ -759,51 +776,96 @@ void calc_navigation_measurement(u8 n_channels, channel_measurement_t meas[], ephemerides_ptrs[i] = &ephemerides[meas[i].sid.sat]; } - calc_navigation_measurement_(n_channels, meas_ptrs, nav_meas_ptrs, nav_time, ephemerides_ptrs); + calc_navigation_measurement_(n_channels, meas_ptrs, nav_meas_ptrs, + nav_time_rx, nav_time, ephemerides_ptrs); } -void calc_navigation_measurement_(u8 n_channels, channel_measurement_t* meas[], navigation_measurement_t* nav_meas[], double nav_time, ephemeris_t* ephemerides[]) +/** Calculate observations from tracking channel measurements. + * + * This function takes an array of pointers, for a version taking a flat array + * see calc_navigation_measurement(). + * + * \param n_channels Number of tracking channel measurements + * \param meas Array of pointers to tracking channel measurements, length + * `n_channels` + * \param nav_meas Array of pointers of where to store the output observations, + * length `n_channels` + * \param nav_time_rx Receiver time at which to calculate the position solution + * in seconds + * \param nav_time Pointer to GPS time at which to calculate the position + * solution. Can be `NULL` in which case one of the + * pseudoranges is chosen as a reference and set to a nominal + * range, implying a certain receiver clock error + * \param ephemerides Array of pointers to ephemerides + */ +void calc_navigation_measurement_(u8 n_channels, channel_measurement_t* meas[], + navigation_measurement_t* nav_meas[], + double nav_time_rx, gps_time_t *nav_time, + ephemeris_t* ephemerides[]) { - double TOTs[n_channels]; - double min_TOF = -DBL_MAX; double clock_err[n_channels], clock_rate_err[n_channels]; for (u8 i=0; itime_of_week_ms; - TOTs[i] += meas[i]->code_phase_chips / 1.023e6; - TOTs[i] += (nav_time - meas[i]->receiver_time) * meas[i]->code_phase_rate / 1.023e6; - - /** \todo Maybe keep track of week number in tracking channel - state or derive it from system time. */ - nav_meas[i]->tot.tow = TOTs[i]; + nav_meas[i]->sid = meas[i]->sid; + + /* Compute the time of transmit of the signal on the satellite from the + * tracking loop parameters. This will be used to compute the pseudorange. */ + nav_meas[i]->tot.tow = 1e-3 * meas[i]->time_of_week_ms; + nav_meas[i]->tot.tow += meas[i]->code_phase_chips / 1.023e6; + nav_meas[i]->tot.tow += (nav_time_rx - meas[i]->receiver_time) + * meas[i]->code_phase_rate / 1.023e6; + /* For now use the week number from the ephemeris. */ + /* TODO: Should we use a more reliable source for the week number? */ gps_time_match_weeks(&nav_meas[i]->tot, &ephemerides[i]->toe); - nav_meas[i]->raw_doppler = meas[i]->carrier_freq; - nav_meas[i]->snr = meas[i]->snr; - nav_meas[i]->sid.sat = meas[i]->sid.sat; - + /* Compute the carrier phase measurement. */ nav_meas[i]->carrier_phase = meas[i]->carrier_phase; - nav_meas[i]->carrier_phase += (nav_time - meas[i]->receiver_time) * meas[i]->carrier_freq; + nav_meas[i]->carrier_phase += (nav_time_rx - meas[i]->receiver_time) + * meas[i]->carrier_freq; + /* For raw Doppler we use the instantaneous carrier frequency from the + * tracking loop. */ + nav_meas[i]->raw_doppler = meas[i]->carrier_freq; + + /* Copy over remaining values. */ + nav_meas[i]->snr = meas[i]->snr; nav_meas[i]->lock_counter = meas[i]->lock_counter; - /* calc sat clock error */ + /* Calculate satellite position, velocity and clock error */ calc_sat_state(ephemerides[i], nav_meas[i]->tot, nav_meas[i]->sat_pos, nav_meas[i]->sat_vel, &clock_err[i], &clock_rate_err[i]); + } - /* remove clock error to put all tots within the same time window */ - if ((TOTs[i] + clock_err[i]) > min_TOF) - min_TOF = TOTs[i]; + /* To calculate the pseudorange from the time of transmit we need the local + * time of reception. */ + gps_time_t tor; + if (nav_time) { + /* If we were given a time, use that. */ + tor = *nav_time; + } else { + /* If we were not given a recieve time then we can just set one of the + * pseudoranges aribtrarily to a nominal value and reference all the other + * pseudoranges to that. This doesn't affect the PVT solution but does + * potentially correspond to a large receiver clock error. */ + tor = nav_meas[0]->tot; + tor.tow += GPS_NOMINAL_RANGE / GPS_C; + tor = normalize_gps_time(tor); } for (u8 i=0; iraw_pseudorange = (min_TOF - TOTs[i])*GPS_C + GPS_NOMINAL_RANGE; - - nav_meas[i]->pseudorange = nav_meas[i]->raw_pseudorange \ - + clock_err[i]*GPS_C; - nav_meas[i]->doppler = nav_meas[i]->raw_doppler + clock_rate_err[i]*GPS_L1_HZ; - + /* The raw pseudorange is just the time of flight divided by the speed of + * light. */ + nav_meas[i]->raw_pseudorange = GPS_C * gpsdifftime(tor, nav_meas[i]->tot); + + /* The corrected pseudorange and Doppler applies the clock error and clock + * rate error correction from the ephemeris respectively. */ + nav_meas[i]->pseudorange = nav_meas[i]->raw_pseudorange + + clock_err[i] * GPS_C; + nav_meas[i]->doppler = nav_meas[i]->raw_doppler + + clock_rate_err[i] * GPS_L1_HZ; + + /* We also apply the clock correction to the time of transmit. */ nav_meas[i]->tot.tow -= clock_err[i]; nav_meas[i]->tot = normalize_gps_time(nav_meas[i]->tot); } From f02f332e5e95ce2657666caf4d12db5a524cc0d1 Mon Sep 17 00:00:00 2001 From: Fergus Noble Date: Wed, 18 Nov 2015 00:16:50 -0800 Subject: [PATCH 2/2] Initial hatch filter implementation --- include/libswiftnav/signal.h | 8 ++++++ include/libswiftnav/track.h | 13 +++++++++ src/track.c | 55 ++++++++++++++++++++++++++++++++++++ 3 files changed, 76 insertions(+) diff --git a/include/libswiftnav/signal.h b/include/libswiftnav/signal.h index 80e6884a..a53254e2 100644 --- a/include/libswiftnav/signal.h +++ b/include/libswiftnav/signal.h @@ -26,6 +26,7 @@ #define CONSTELLATION_GPS 0 #define CONSTELLATION_SBAS 1 +#define CONSTELLATION_INVALID 255 #define BAND_L1 0 @@ -55,5 +56,12 @@ static inline bool sid_is_equal(const gnss_signal_t a, const gnss_signal_t b) return sid_compare(a, b) == 0; } +/** Sets a signal ID such that it will not compare equal with any valid signal + * ID. */ +static inline void sid_mark_invalid(gnss_signal_t *s) +{ + s->constellation = CONSTELLATION_INVALID; +} + #endif /* LIBSWIFTNAV_SIGNAL_H */ diff --git a/include/libswiftnav/track.h b/include/libswiftnav/track.h index 5f00fd00..c777e4b5 100644 --- a/include/libswiftnav/track.h +++ b/include/libswiftnav/track.h @@ -172,6 +172,15 @@ typedef struct { u16 lock_counter; } navigation_measurement_t; +typedef struct { + double last_pseudorange; + double last_carrier_phase; + gnss_signal_t sid; + u16 last_lock_counter; +} hatch_state_t; + +#define HATCH_N_DEFAULT 5 + void calc_loop_gains(float bw, float zeta, float k, float loop_freq, float *b0, float *b1); float costas_discriminator(float I, float Q); @@ -231,6 +240,10 @@ void cn0_est_init(cn0_est_state_t *s, float bw, float cn0_0, float cutoff_freq, float loop_freq); float cn0_est(cn0_est_state_t *s, float I, float Q); +void hatch_filter_init(hatch_state_t *s, u8 ns); +void hatch_filter_update(hatch_state_t *s, u8 ns, double N, u8 nm, + navigation_measurement_t *nms); + void calc_navigation_measurement(u8 n_channels, channel_measurement_t meas[], navigation_measurement_t nav_meas[], double nav_time_tc, gps_time_t *nav_time, diff --git a/src/track.c b/src/track.c index 3e73e392..c480cab8 100644 --- a/src/track.c +++ b/src/track.c @@ -14,6 +14,7 @@ #include #include #include +#include #include "constants.h" #include "prns.h" @@ -21,6 +22,8 @@ #include "ephemeris.h" #include "tropo.h" #include "coord_system.h" +#include "signal.h" +#include "set.h" /** \defgroup track Tracking * Functions used in tracking. @@ -745,6 +748,58 @@ float cn0_est(cn0_est_state_t *s, float I, float Q) return s->log_bw - 10.f*log10f(s->nsr); } + +void hatch_filter_init(hatch_state_t *s, u8 ns) +{ + for (u8 i=0; isid, nm->sid)); + + nm->pseudorange = (1 / N) * nm->pseudorange + + ((N - 1) / N) * (s->last_pseudorange + + nm->carrier_phase + - s->last_carrier_phase); +} + +static int cmp_hatch_nm(const void *a, const void *b) +{ + navigation_measurement_t *nm = (navigation_measurement_t *)a; + hatch_state_t *s = (hatch_state_t *)b; + return sid_compare(nm->sid, s->sid); +} + +void hatch_filter_update(hatch_state_t *s, u8 ns, double N, u8 nm, + navigation_measurement_t *nms) +{ + assert(ns >= nm); + + intersection_map(nm, sizeof(navigation_measurement_t), nms, + ns, sizeof(hatch_state_t), s, + cmp_hatch_nm, &N, hatch_intersection_f); + + u8 i; + for (i=0; i